高精度IMU精对准原理和代码实现

---

前言

---

高精度imu经过初始对准后，获得了载体坐标b系到导航坐标系n的粗略姿态矩阵Cnb，但还存在一定的误差，若此时直接进行纯惯性导航，误差会迅速发散，因此，需要共进一步求准Cnb。

一、基于速度误差的精对准

速度误差的精对准，可以理解为在静止的时刻进行零速约束，对常规的姿态和速度更新算法进行简化（此时位置不变，不需要更新）
对误差方程进行简化得：
最终构建以EN方向速度误差为观测量，3个失准角误差、EN方向速度误差、NU方向的陀螺零偏误差为状态量的滤波模型，如下：

二、代码实现

上传在github的c++工程文件，有数据可以运行，代码包含粗对准和精对准2部分。

链接: https://github.com/shaoudelaochao/ImuAlignment/tree/master/ImuAlignment

结果分析

1、姿态角及其协方差图：
水平姿态角很容易收敛，航向角收敛慢一点（跟初始给的P0设置有关），有个pitch角慢慢漂了，说明这个轴的零偏稳定性较其他2个轴差一点。

2、速度及其协方差
速度误差是直接观测量，收敛也很快。

3、零偏及其协方差
N方向的零偏有收敛，U方向基本没有收敛，因为u方向零偏误差可观测性最弱（反过来说，如果滤波器中可观测性最弱的状态都收敛了，那整个滤波器就很稳定了）。

7维状态的可观测性理论分析如下：

结论：理论分析跟上面的实验数据结果一致：速度误差是直接观测的，最先收敛；N和E方向的失准角是一次求导算的，慢收敛一点；U方向失准角和N方向零偏误差是二次求导算的，慢慢才收敛，2个收敛曲线一致；U方向零偏误差是三次求导的，收敛最慢。