![]()
functioncompare_dcm(C1,C2,name1,name2)% COMPARE_DCM 比较两个方向余弦矩阵的误差% 输入:% C1 - 第一个DCM矩阵 (3x3)% C2 - 第二个DCM矩阵 (3x3)% name1 - 第一个矩阵的名称 (字符串)% name2 - 第二个矩阵的名称 (字符串)R2D=180/pi;% 计算相对旋转矩阵C_delta=C1*C2';% 等效旋转角[theta_deg,theta_rad]=dcm2angle_trace(C_delta);% 欧拉角 (ZYX 321顺序)，rotm2eul 返回 [psi, theta, phi] 的 1×3 行向量eul=rotm2eul(C_delta,'ZYX');% 返回 1x3: [psi, theta, phi]psi_deg=eul(1)*R2D;% 方位theta_e=eul(2)*R2D;% 俯仰phi_deg=eul(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');end
importnumpyasnpdefrotation_matrix_angle_error(R1,R2):""" 计算两个3x3旋转矩阵之间的姿态夹角误差 :param R1: 基准旋转矩阵 (3x3 numpy array) :param R2: 待评估旋转矩阵 (3x3 numpy array) :return: 误差角 (度), 误差角 (角秒) """# 计算相对旋转矩阵R_rel=R2 @ R1.T# 计算矩阵迹tr=np.trace(R_rel)# 计算旋转角（弧度）theta_rad=np.arccos((tr-1)/2)# 转换为角度和角秒theta_deg=np.rad2deg(theta_rad)theta_arcsec=theta_deg*3600returntheta_deg,theta_arcsec# 基准矩阵R_base=np.array([[0.369570943258714,0.929183591370813,-0.005855345436375],[0.928725095037648,-0.369226681941889,0.025702187590948],[0.021718859664471,-0.014940172615175,-0.999411929307825]])# 计算误差deg_A,arcsec_A=rotation_matrix_angle_error(R_base,R_A)deg_B,arcsec_B=rotation_matrix_angle_error(R_base,R_B)deg_C,arcsec_C=rotation_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}'')")
