惯性导航学习过程记录（姿态更新，计算失准角）

1. 利用旋转矩阵进行姿态更新

function Cb2n = ch_mnormlz(Cb2n)
% 姿态阵单位化，防止矩阵蠕变
    for k=1:5
%         Cnb = 0.5 * (Cnb + (Cnb')^-1);  % Algorithm 1 
        Cb2n = 1.5*Cb2n - 0.5*(Cb2n*Cb2n')*Cb2n;  % Algorithm 2 
    end
    
%   Algorithm 3:  [s,v,d] = svd(Cnb); Cnb = s*d';  % in = s*v*d' = s*d' * d*v*d';  out = s*d'

clear;
clc
close all;

%% 已知信息
w = [2 -3 5]';
dt = 0.01; %姿态更新周期: 0.01s = 100Hz
dur = 100; %积分时长 单位s


N = dur / dt; %积分次数
eul = zeros(N, 3);
fprintf("已知b系角速度(陀螺仪输出为):%.3fdeg/s %.3fdeg/s %.3fdeg/s\n", w(1), w(2), w(3));

%定轴转动下: 等效旋转矢量 = 角增量 = 角速度*dt
Cb2n = eye(3);
theta = deg2rad(w)*dt; 


for i = 1:N
    C_m2m_1 = ch_rv2m(theta);  % 等效旋转矢量转成旋转矩阵
    Cb2n = Cb2n * C_m2m_1;

    
    %单位阵正交化
    Cb2n = ch_mnormlz(Cb2n);

    %记录每一步的欧拉角，姿态阵转成欧拉角
    eul(i,:) = rad2deg(ch_m2eul(Cb2n));
end


plot(eul)
legend("PITCH(deg)", "ROLL(deg)", "YAW(deg)");

tmp =eul(end,:);
fprintf("最终欧拉角: pich:%.4f° roll:%.4f° yaw:%.4f°\n", tmp(1), tmp(2), tmp(3));

运行结果：

2. 利用四元数进行姿态更新

clear;
clc
close all;

%% 已知
w = [1 1 0]'; %角速度
dt = 0.01; %姿态更新周期: 0.01s = 100Hz
dur = 10; %积分时长 单位s


N = dur / dt; %积分次数
eul = zeros(N, 3);
fprintf("已知b系角速度(陀螺仪输出为):%.3fdeg/s %.3fdeg/s %.3fdeg/s\n", w(1), w(2), w(3));


Qb2n = [1 0 0 0]';
theta = deg2rad(w)*dt; %定轴转动下: 等效旋转矢量 = 角增量 = 角速度*dt


for i = 1:N
    Q_m2m_1 = ch_rv2q(theta);
    
    Qb2n  = ch_qmul(Qb2n, Q_m2m_1);
    
    %四元数单位化
    Qb2n = ch_qnormlz(Qb2n);

    %记录每一步的欧拉角
    eul(i,:) = rad2deg(ch_q2eul(Qb2n));
end


plot(eul)
legend("PITCH(deg)", "ROLL(deg)", "YAW(deg)");

tmp =eul(end,:);
fprintf("最终欧拉角: pich:%.4f° roll:%.4f° yaw:%.4f°\n", tmp(1), tmp(2), tmp(3));

运行结果：

3. 失准角计算方法

失准角使用姿态矩阵计算的！！！

clear;
clc
close all;
format 

%% 一个简单的失准角例子

% M_gen = ch_eul2m(deg2rad([30,40,50.2]))

Cb2n = [
    0.2462   -0.6634    0.7066
    0.7934    0.5567    0.2462
   -0.5567    0.5000    0.6634
];

Cb2np = [
    0.2434   -0.6654    0.7057
    0.7943    0.5544    0.2487
   -0.5567    0.5000    0.6634
    ];

fprintf("Cb2n: 真实姿态阵):\n");
fprintf("Cb2np: 计算导航系姿态阵(有误差的)):\n");


fprintf("失准角反对称阵为:\n");
phi_skew = -(Cb2np * Cb2n'  - eye(3));
phi_skew


phi = [-phi_skew(2,3), phi_skew(1,3), phi_skew(1,2)];
phi = rad2deg(phi);
fprintf("失准角:%f %f %f (deg)\n", phi(1), phi(2), phi(3));

运行结果：

Cb2n: 真实姿态阵):
Cb2np: 计算导航系姿态阵(有误差的)):
失准角反对称阵为:

phi_skew =

    0.0000    0.0036    0.0000
   -0.0035   -0.0001   -0.0000
    0.0000    0.0000   -0.0000

失准角:0.000044 0.002257 0.204619 (deg)