惯性导航学习过程记录（2d导航）

clear;
clc
close all;

%%
% 《全球导航卫星系统、惯性和多传感器综合导航系统原理》，第2版，Paul D.Groves著
% EXAMPLE 5.1
% 二维惯性导航

%% INTPUS 
p = [10;  10];     % 初始位置
v = [0;  0];    % 初始速度
y = deg2rad(30);   % 初始角度


w = [0; 0; 0; 0];              % 角速度输入
dt = 1;                           % 积分间隔

% 加速度输入
a = [0   2;        % 北向加速
       1  -2;        % 东向加速,北向减速，
      -1  -2 ;        % 向南开
       -1   2];        

p_l = zeros(4,2);     %记录位置
p_l(1,:)= p; 
for i = 1:4

y_ = y;               % 上一次的航向角
y = y + w(i)*dt;  % 更新航向角

% 将加速度转换为p帧，得到平均值
Cnb_ = [cos(y_) -sin(y_); sin(y_) cos(y_)];
Cnb = [cos(y) -sin(y); sin(y) cos(y)];
Cnb = (Cnb_ + Cnb) / 2;

a_n = Cnb*a(i,:)'; %N系下加速度

%更新速度
v_ = v; 
v = v + a_n*dt;

%更新位置
p = p + (v_ +v)*0.5*dt;
p_l(i+1,:) = p ;

end

fprintf("最终位置:(m)\n");
p

figure;
plot(p_l(:,1),p_l(:,2));
title("2d 导航");

 运行结果：

碎碎念~：  

        2d导航的加速度和航向角总弄混，把它想想成开车就容易多了，车身有一个y向的速度用于控制前进后退，有一个x向的速度控制左右转弯，航向角是决定你的车头朝向那。你的视角是载体系b系，对于车身来讲，哪个位置又加速了；站在楼上正在看你的老妈的视角是n系，她可以知道你的车头朝向，和现在所在的位置。

        和开车有一点不同，汽车不踩油门，不给加速度就会停；代码里则是需要给个反向加速度，速度才能减速，要不就保持原来速度。