比较两个旋转矩阵的误差

functioncompare_dcm(C1,C2,name1,name2)% COMPARE_DCM 比较两个方向余弦矩阵的误差% 输入:% C1 - 第一个DCM矩阵 (3x3)% C2 - 第二个DCM矩阵 (3x3)% name1 - 第一个矩阵的名称 (字符串)% name2 - 第二个矩阵的名称 (字符串)R2D180/pi;% 计算相对旋转矩阵C_deltaC1*C2;% 等效旋转角[theta_deg,theta_rad]dcm2angle_trace(C_delta);% 欧拉角 (ZYX 321顺序)rotm2eul 返回 [psi, theta, phi] 的 1×3 行向量eulrotm2eul(C_delta,ZYX);% 返回 1x3: [psi, theta, phi]psi_degeul(1)*R2D;% 方位theta_eeul(2)*R2D;% 俯仰phi_degeul(3)*R2D;% 滚转fprintf(\n 矩阵误差分析: [%s] vs [%s] \n,name1,name2);fprintf(等效旋转角: %.6f 度 (%.8f rad)\n,theta_deg,theta_rad);fprintf(方位角误差 ψ: %.6f 度\n,psi_deg);fprintf(俯仰角误差 θ: %.6f 度\n,theta_e);fprintf(滚转角误差 φ: %.6f 度\n,phi_deg);fprintf(\n);endimportnumpyasnpdefrotation_matrix_angle_error(R1,R2): 计算两个3x3旋转矩阵之间的姿态夹角误差 :param R1: 基准旋转矩阵 (3x3 numpy array) :param R2: 待评估旋转矩阵 (3x3 numpy array) :return: 误差角 (度), 误差角 (角秒) # 计算相对旋转矩阵R_relR2 R1.T# 计算矩阵迹trnp.trace(R_rel)# 计算旋转角弧度theta_radnp.arccos((tr-1)/2)# 转换为角度和角秒theta_degnp.rad2deg(theta_rad)theta_arcsectheta_deg*3600returntheta_deg,theta_arcsec# 基准矩阵R_basenp.array([[0.369570943258714,0.929183591370813,-0.005855345436375],[0.928725095037648,-0.369226681941889,0.025702187590948],[0.021718859664471,-0.014940172615175,-0.999411929307825]])# 计算误差deg_A,arcsec_Arotation_matrix_angle_error(R_base,R_A)deg_B,arcsec_Brotation_matrix_angle_error(R_base,R_B)deg_C,arcsec_Crotation_matrix_angle_error(R_base,R_C)print(f基准矩阵 vs 精测矩阵A:{deg_A:.3f}° ({arcsec_A:.1f}))print(f基准矩阵 vs 精测矩阵B:{deg_B:.3f}° ({arcsec_B:.1f}))print(f基准矩阵 vs 精测矩阵C:{deg_C:.3f}° ({arcsec_C:.1f}))