惯性导航(一）

本文以二维为例，讲解惯性导航（IMU与GPS传感器融合）。

一，坐标系与数据

如下图所示为GPS的轨迹图，（运行utility/showgps.m), 横坐标x为东向，纵坐标为北向．

如下图所示为imu的波形图，可以看出50s处（从上图中看出，车辆右转），角速度为负，局部坐标系应是x朝右，ｙ超前，顺时针为负．

二，惯性导航方程

惯性导航的三大优势:

1. 不受外界干扰
2. 高频
3. ６自由度估计

惯性导航方程描述的是估计变量与IMU量测值之间的关系，首先将加速度计观测到的局部观测值，转换到全局坐标系下去：
$a^G = Ra^L$
其中 $a^L$是2D的局部加速度，R是姿态．陀螺仪的观测值角速度并没有全局局部之分，直接可以使用时间，将角度的增量求出来：
$\delta \theta = (yaw_{rate} + yaw_{bias})*\delta t$

• 姿态的更新
  $\theta_t = \theta_{t-1} + \delta \theta$
• 速度的更新
  $v_t = v_{t-1} + a^G\delta t = v_{t-1} + R*a^L\delta t$
• 位置的更新
  $s_t = s_{t-1} + v_{t-1}\delta t + 1/2 a^G \delta t^2 = s_{t-1} + v_{t-1}\delta t + 1/2 R*a^L \delta t^2$
• bias的更新
  $bias_{t} = bias_{t-1}$
  其中变量维度：姿态，速度，位置，陀螺仪bias, 维度为1+2+2+1共6维

三，Prediction in EKF

状态的更新直接使用惯性导航方程
$x = [\theta, v, s, bias]^T$
$\bar{x}(t) = f(x(t-1)) + u$
$\bar{P} = F^TPF + Q$
其中P是一个66的协方差矩阵，f函数的输入是61, 输出为61, u是一个0为均值，Q为方差的过程噪声．F是一个66的雅克比矩阵：
$F = \frac{\partial f}{\partial x } \\ = \left[ \begin{matrix}1 & 0 &0 &0 &0&\delta t \\ \left[ \begin{matrix}-\sin(\theta)& -\cos(\theta)\\ \cos(\theta)&-\sin(\theta)\end{matrix}\right ]a^L\delta t &1&0&0&0&0\\ ...&0&1&0&0&0\\ 1/2 \left[ \begin{matrix}-\sin(\theta)& -\cos(\theta)\\ \cos(\theta)&-\sin(\theta)\end{matrix}\right ]a^L\delta t^2 &I_{2*2}\delta t &...&I_{2*2}&...&0\\ 0 & 0 &0 &0 &0&1 \end{matrix}\right ]$

四，Update using GPS

在组合惯性导航中，GPS是一个非常理想的可以与ＩＭＵ配合使用的器件．IMU以100hz输出信息，GPS以10HZ输出信息．具体的，在ＥＫＦ中，每次ＧＰＳ的观测作为measurement包含到整体系统中．更新三板斧：计算残差，计算卡尔曼增益，计算状态量与方差．
首先残差可以定义为：
$y = z - H\bar{x}$
其中z为gps的21维的观测值， $\bar{x}$为状态向量．Ｈ为26观测矩阵：
$H = \left[ \begin{matrix}0&0&0&1&0&0\\0&0&0&0&1&0\end{matrix}\right]$
卡尔曼增益（5*2)如下：
$K = PH^T(HPH^T + R)^{-1}$
计算状态量：
$x = \bar{x} + Ky$
计算方差
$P = (I - KH)\bar{P}$

五，实验

状态转换函数f(x)以及其雅克比F定义如下：

theta_t = theta_t_1 + yawrate*delta_t;
v_t = v_t_1 + to_R2d(theta_t_1)*a_L*delta_t;
s_t = s_t_1 + v_t_1*delta_t+ 1/2*to_R2d(theta_t_1)*a_L*delta_t*delta_t;
fx =[theta_t;v_t;s_t];

dR_theta = [-sin(theta_t_1),-cos(theta_t_1);cos(theta_t_1),-sin(theta_t_1)];
F = eye(5,5);
F(2:3, 1) = dR_theta*a_L*delta_t;
F(4:5, 1) =1/2*dR_theta*a_L*delta_t*delta_t;
F(4:5, 2:3) = eye(2,2)*delta_t;

预测部分：

[X_bar, F] = get_state_transition_F(delta_t,yaw_rate,aL,X(1),X(2:3),X(4:5));
Q  = get_Q();
P_bar = F'*P*F + Q;

更新部分：

H = zeros(2,5);
H(1:2,4:5) = eye(2,2);
R = 4*eye(2,2);
z = data2d.GNSS.pos_EN(:,it_gps);
y = z - H*X_bar;
K = P_bar*H'*inv(H*P_bar*H' + R);
X = X_bar + K*y;
P = (eye(5,5) - K*H)*P_bar;

实验得到如下位置结果：

可以看出方向大致正确，因为GPS 1HZ的原因，导致在接收GPS的瞬间，并不是特别连续．
代码见：github 2d case. run: run_kf_INS.m.

接着使用tutlebot仿真数据，该数据有三个优点：1，IMU数据漂移较小。2，GPS为10HZ，3，坐标轴无歧义。
全局轨迹结果如下，其中红色为gps观测，蓝色为imu积分轨迹

将一处转弯处放大的结果如下：