惯性导航(三）估计偏置

真实工况下IMU都会有较大的偏置，偏置一般与温度、出厂精度、实际运动动态程度。在位姿估计中，会有累积效应，因此估计偏置是惯性导航中的常用做法。

一、状态时空模型

1、状态向量定义、预测模型

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

• 位置的更新
  $$p_t=p_{t-1}+v_{t-1}\delta t+1/2a^G\delta t^2$$
• 速度的更新
  $$v_t=v_{t-1}+a^G\delta t$$
• 姿态的更新
  $${\theta }_t={\theta }_{t-1}+\delta \theta$$
• bias的更新
  $$a_{b(t)}=a_{b(t-1)}$$
  $${\theta }_{b(t)}={\theta }_{b(t-1)}$$
  综上，状态向量组成：PVQ,Bias，一共8维. 根据EKF的标准形式：${\bar{X}}_t=f(X_{t-1},u)$, $X=[p,v,\theta ,a_b,{\theta }_b]$.

2、协方差部分

非线性模型f的对$X_{t-1}$的偏导数形成转换矩阵F.

• 其关于位置的部分：
  $$F_{1-2}=[\begin{array}{ccccccc}1& 0& \delta t& 0& \frac{\partial p}{\partial \theta }& \frac{\partial p}{\partial a_b}& \frac{\partial p}{\partial {\theta }_b}\\ 0& 1& 0& \delta t& -& -& -\end{array}]$$
  其中：
  $\frac{\partial p}{\partial \theta }=0.5R^{\prime }(a^L+a_b)\delta t^2$, $\frac{\partial p}{\partial a_b}=0.5R[\begin{array}{c}1\\ 1\end{array}]\delta t^2$，$\frac{\partial p}{\partial {\theta }_b}=0.5R^{\prime }(a^L+a_b)\delta t^2？$（应该是0）, $R^{\prime }=[\begin{array}{cc}-sin\theta & -cos\theta \\ cos\theta & -sin\theta \end{array}]$
• 关于速度部分：
  $$F_{3-4}=[\begin{array}{ccccccc}0& 0& 1& 0& \frac{\partial v_t}{\partial {\theta }_{t-1}}& \frac{\partial v_t}{\partial a_b}& 0\\ 0& 0& 0& 1& -& -& 0\end{array}]$$
  其中：$\frac{\partial v_t}{\partial {\theta }_{t-1}}=R^{\prime }(a^L+a_b)\delta t$, $\frac{\partial v_t}{\partial a_b}=R[1,1{]}^T\delta t$.
• 关于姿态部分：
  $$F_5=[\begin{array}{cccccccc}0& 0& 0& 0& 1& 0& 0& \delta t\end{array}]$$
• 关于加速度/角速度偏置部分：
  $$F_{6-8}=[\begin{array}{cccccccc}0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}]$$

二、量测更新部分

这里先引入GPS作为观测信息，根据$y=z-H\bar{x}$，可知:
$$H=[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\end{array}]$$

三、实验结果

实验步骤：

1. 运行数据生成脚本：/media/m0/Data/code/gnss-ins-sim/demo_no_algo.py，轨迹配置文件/media/m0/Data/code/gnss-ins-sim/demo_motion_def_files/motion_def-2d_long.csv，IMU配置文件/media/m0/Data/code/gnss-ins-sim/gnss_ins_sim/sim/imu_model.py
2. 运行算法运行脚本：/home/m0/code/INS/2d_case/04_gnss_sim_bias/run_gnss_imu_sim.m

1. (加速度bias)真实数据$a_b^x=0.0$, 初始值也为$a_b^x=0.0$;

可以看出估计出来的bias也为0，符合预期。

2. (加速度bias)真实数据$a_b^x=0.0$，初始为$a_b^x=-0.1$.

从左下角的bias估计可以看出，从-0.1收敛到0附近，符合预期。

3. (加速度bias)真实数据$a_b=[0.1,0.1{]}^T$，初始为$a_b^x=[0,0{]}^T$.

可以看出，bias收敛到真值附近。

4. (角速度bias)真实数据${\theta }_b=0$, 初始值为${\theta }_b=-0.03$

可以看到航向角收敛至0，但是也影响到了加速度计的偏置估计，对于协方差的调优将在后续文章中解释。

参考文献

1. 一个通俗的博客