比较两个旋转矩阵的误差

matlab 复制代码

function compare_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

python 复制代码

import numpy as np

def rotation_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 * 3600
    return theta_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}'')")