对载体准确的姿态估计是实现高精度导航的前提条件。在消费级产品的导航方案中，无人机需要准确确定当前点头角与侧滚角，进而控制系统实现姿态自稳，是无人机安全行驶至关重要的条件；在高精度导航方案中，惯导系统需在静止状态下需经几分钟至几十分钟的初始对准，该过程就是计算IMU当前的姿态角（也叫作失准角），在长时间对准过程中，Kalman滤波器逐渐达到收敛状态。

在GNSS不参与的姿态估计时，更多依赖于IMU的自修正，主要有三种模式：

① 加速度计的重力加速度分量，用于修正IMU的点头和侧滚方向角度；

② 磁力计感受到的地球磁场方向，用于修正摇头角；

③ 高精度陀螺仪能够测量地球自转的前提下，可以用于摇头角度的初始对准；

其中①和②常见于低精度的导航方案，③见于高精度惯性导航方案，要求陀螺仪的精度高于地球自转角速度（地球自转：15°/h），②不适用于高精度导航方案，因为磁场较为容易受到外界环境的干扰，观测得到的摇头角的精度往往低于陀螺仪的理论精度。不失一般性，下面的内容仅讨论①。

1. 姿态确定方法

姿态估计常采用”error-state EKF“框架，相比于传统的EKF而言，用error state 可降低非线性系统在线性化过程中的误差，是惯性导航中最为常见的方案，其核心思想是用姿态误差的协方差矩阵来描述系统的不确定度。（参考文献：Quaternion kinematics for the error-state KF）（描述不一定准确）在详细设计导航方案时，总结有下面5个步骤：

① 确定待估计的状态量和误差状态量，确定状态更新方程；

② 根据状态更新方程，推导出误差传播方程，并得到kalman滤波的转移矩阵（离散更新方程对误差状态的雅克比矩阵）和协方差更新方程，是其中的核心；

③ 确定多传感器融合的观测模型，并对观测模型推导出残差相对于误差状态的雅克比矩阵；

④ 根据各传感器的误差大小，确定协方差矩阵Q（预测更新矩阵）和R（观测矩阵）

⑤ 确定kalman滤波的更新方程；

状态量就是IMU本体坐标系到导航坐标系下的旋转矩阵（姿态角）和陀螺仪的bias，表示为

2. 右乘扰动

3. 左乘扰动

4. 调参Q，R矩阵

5. 试验结果对比

试验采用经典的高精度MEMS传感器STIM300型号IMU，陀螺仪的零偏稳定性指标为1°/h，加速度计的零偏为1mg，通过RS422传输数据至ARM计算平台，输出帧率为1000Hz，加速度计数据作101个点矩形窗的平滑，每20ms作一次滤波更新。

IMU的初始欧拉角度为0,0,0，IMU中途经过多次摆动和振动操作，历经2个小时之后，对比如下图所示，由于直接积分用旋转矢量泰勒展开近似的模式进行计算，发现仍存在较为明显的误差，实际情况下，STIM300的漂移程度应远低于图中所呈现的那样。ESKF（采用左乘扰动的模式）在点头、侧滚角上表现较好，摇头角表现较差。摇头角的漂移程度大于直接积分的方式，这是因为单矢量观测过程中可能对摇头角进行错误的观测修正。

而右乘扰动的模式中，摇头角度的漂移更大，甚至到2-3s漂移到1°，在对航向角度缺乏有效观测的条件下，摇头角的漂移程度让人吃惊。

左乘扰动

右乘扰动

6. 原因分析

对于为什么左乘扰动中摇头角漂移更小，这是因为将误差状态$\delta \mathbf{\theta }$由本体坐标系改为定义在惯性坐标系。而**王炜鑫**博士认为将姿态误差定义到惯性坐标系后，线性化的不精确就不会对不可观测的自由度做出错误的修正，详细内容可以参考他的博客。因此在高精度的惯性导航中，常采用左乘扰动的方式进行误差传播方程的推导，在精度要求不太高的场景中，相关论文中也有对右乘扰动模型的使用。