
function out = att_upt(in, gyr, dt)
% 输入当前姿态四元数和陀螺仪角速度，输出更新后的姿态四元数
% in为当前姿态四元数
% gyr是陀螺仪测量值
% dt 时间步长

%% 单子样旋转矢量
 rv = gyr*dt;       % 旋转矢量 = 角速度 × 时间步长
 dq = rv2q(rv);  % 调用函数将旋转矢量转为四元数 

%% 不专业的做法
%                  dq(1) = 1;
%                  dq(2) = rv(1)*0.5;
%                  dq(3) = rv(2)*0.5;
%                  dq(4) = rv(3)*0.5;

 out = qmul(in, dq); % 四元数相乘 当前姿态 × 增量 = 新姿态
 out = qnormlz(out); % 归一化 归一化防止数值漂移

 %% 使用旋转矩阵更新
% 
%  Cb2n = ch_q2m(in);
%  theta = gyr*dt;
% 
% %C = eye(3) + ch_askew(theta);
% C = ch_rv2m(theta);
% 
% Cb2n = Cb2n * C;
% 
% % 截断误差，保持正交化 GNSS与惯性及多传感器组合导航系统原理-第二版.pdf 公式 5.80
% c1 = Cb2n(1,:);
% c2 = Cb2n(2,:);
% c3 = Cb2n(3,:);
% c1 = 2 / (1 + dot(c1,c1))*c1;
% c2 = 2 / (1 + dot(c2,c2))*c2;
% c3 = 2 / (1 + dot(c3,c3))*c3;
% Cb2n = [c1; c2; c3];
% 
% out = ch_m2q(Cb2n);


end

