比较两个旋转矩阵的误差

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}'')")
相关推荐
tellmewhoisi12 小时前
深度学习0:数学知识:(雅可比矩阵)
人工智能·深度学习·矩阵
lisw057 天前
Dayhoff的PAM替换矩阵理论评介!
线性代数·算法·矩阵
DFT计算杂谈7 天前
可DIY布里渊区图像绘制编辑器
人工智能·机器学习·语言模型·矩阵·编辑器
Pniubi7 天前
力扣54.螺旋矩阵
算法·leetcode·矩阵
Pniubi8 天前
力扣73 矩阵置零
算法·leetcode·矩阵
kingx39 天前
Bugku 逆向 MatrixHunter:黑盒 oracle + GF(2) 消元出唯一解
网络安全·矩阵·逆向·ctf
莱茶荼菜13 天前
【slam公式】
人工智能·线性代数·机器学习
吴名氏.13 天前
Java转AI Agent教程(第九周)错误矩阵
java·人工智能·矩阵·ai agent
伯奥克客服14 天前
2026年视频矩阵厂家技术评估:从EDID到控制协议开放度的六维筛选法
线性代数·矩阵·音视频