forked from ZhanLi/UWBIns
61 lines
1.5 KiB
Mathematica
61 lines
1.5 KiB
Mathematica
|
function [p, v, q] = nav_equ_local_tan(p, v, q ,acc, gyr, dt, gN)
|
|||
|
|
% 惯导解算更新，当地直角坐标系，不考虑地球自转
|
|||
|
|
% p 位置 XYZ 单位 m
|
|||
|
|
% v 速度 XYZ 单位 m/s
|
|||
|
|
% q Qb2n姿态,四元数表示
|
|||
|
|
% acc 比力， 加速度计测量值 单位 (m/s^2),
|
|||
|
|
% gyr 角速度 (rad/s)]
|
|||
|
|
% dt dt (s) 积分间隔如 0.01s
|
|||
|
|
% gn 当地重力向量
|
|||
|
|
|
|||
|
|
old_v = v; % 保存当前速度，用于后续梯形积分
|
|||
|
|
|
|||
|
|
sf = acc; % 比力（Specific Force） = 加速度计测量值（忽略零偏和噪声）
|
|||
|
|
|
|||
|
|
% 姿态结算
|
|||
|
|
q = att_upt(q, gyr, dt); % 通过陀螺仪数据更新四元数
|
|||
|
|
% 输入当前姿态四元数和陀螺仪角速度，输出更新后的姿态四元数
|
|||
|
|
|
|||
|
|
|
|||
|
|
% 速度解算
|
|||
|
|
sf = qmulv(q, sf); % 将比力从机体系转换到导航系
|
|||
|
|
sf = sf + gN; % 添加重力补偿（导航系下）
|
|||
|
|
v = old_v + dt *sf; % 积分加速度得到速度
|
|||
|
|
|
|||
|
|
% 位置解算
|
|||
|
|
p = p + (old_v + v) *dt/2; % 梯形法积分速度
|
|||
|
|
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|
|||
|
|
%
|
|||
|
|
%
|
|||
|
|
% function x = ch_nav_equ_local_tan(x ,u, dt, gN)
|
|||
|
|
%
|
|||
|
|
% persistent a_old;
|
|||
|
|
% if isempty(a_old)
|
|||
|
|
% a_old= u(1:3);
|
|||
|
|
% end
|
|||
|
|
%
|
|||
|
|
% old_v = x(4:6);
|
|||
|
|
%
|
|||
|
|
% a_new =u(1:3);
|
|||
|
|
% %sf = sf + 0.5*cross(u(4:6)*dt, sf);
|
|||
|
|
%
|
|||
|
|
% % 姿态结算
|
|||
|
|
% gyr = u(4:6);
|
|||
|
|
% q_old = x(7:10);
|
|||
|
|
% x(7:10) = ch_att_upt(x(7:10), gyr, dt);
|
|||
|
|
% q_new = x(7:10);
|
|||
|
|
%
|
|||
|
|
% % 速度解算
|
|||
|
|
%
|
|||
|
|
% x(4:6) = old_v + ((ch_qmulv(q_new, a_new) + ch_qmulv(q_old, a_old) )/2 + gN) *dt;
|
|||
|
|
%
|
|||
|
|
% % 位置解算
|
|||
|
|
% x(1:3) = x(1:3) + (old_v + x(4:6)) *dt/2;
|
|||
|
|
% a_old = a_new;
|
|||
|
|
% end
|
|||
|
|
%
|
|||
|
|
%
|