一、简化的惯导算法

 1、姿态更新

陀螺仪输出直接进行积分，得到角增量；

2、速度更新：

在导航系下，完整的速度微分方程，如下：

其中，在地速V<100m/s时，第二项量级大约为1mg；小于传感器本身的误差；因此，忽略地球自转和导航系在地球表面旋转的影响，得到如下更新方程：

3、位置更新：

二、低精度惯导系统误差方程

1、姿态误差方程：

其中，括号中第一项为陀螺仪的一阶马尔可夫过程误差：括号中第二项为陀螺仪角速率白噪声。

其中τGi为相关时间常数，WrGi是一阶马尔可夫过程激励白噪声。

误差方程详细解释：捷联惯导基础知识解析之二（捷联惯导更新算法和误差方程）

噪声详细解释点击：捷联惯导基础知识解析之三（噪声和kalman滤波）

2、速度误差方程：

其中，括号中第一项为加速度计的一阶马尔可夫过程误差：括号中第二项为加速度计比力的白噪声。

其中τAi为相关时间常数，WrAi是一阶马尔可夫过程激励白噪声。

综上，可以看出陀螺仪和加速度计噪声均采用一阶马尔可夫过程；其优点如下：

 Allan方差噪声系数，主要有用的是角度随机游走系数（用于设置Q阵）和零偏不稳定性系数（用于设置一阶马氏过程的方差）

参考链接：Allan方差分析的使用要点

3、位置误差方程：

二、地磁场测量及误差方程

1、地磁场描述：

2、磁场坐标系/磁强计敏感轴坐标系

（1）磁场坐标系（OXYZm,简称m系）：oym指向磁场方向；oxm在水平面内（此时，X轴指向不确定是哪里）；ozm与前两者构成右手直角坐标系；如下所示：

因此，从导航坐标系（n系）到磁场坐标系的变换矩阵为：

实际中，计算磁场坐标系为m'，则m系和m‘系的变换矩阵为：

（2）三轴磁强计敏感轴确定的直角坐标系为bm，其与IMU的b系并不完全一致；

3、磁场测量误差：

注意：在计算中，将磁场测量值进行归一化处理；磁测量误差的最后形式为：

三、低成本组合导航系统模型

IMU/GPS/磁力计组合导航系统中，选取状态量为：

姿态失准角、速度误差、位置误差、陀螺仪相关漂移、加速度计相关漂移、安装偏差角（磁力计与IMU安装偏差角）、磁力计倾角、磁力计偏角；一共20维状态量；

其中，系统状态空间模型为：

三、低成本惯导的姿态初始化

1、利用加速度计进行水平对准

静止条件下，速度为0；因此，其速度微分方程可写为:

其中为了减少加速度计测量噪声和外界晃动干扰加速度的影响，常采用一段时间内的平均比力进行计算：上式经过变换得：

分量形式为：

根据姿态矩阵求取欧拉角公式：

2、利用地磁测量进行方位对准

在忽略磁偏角的情况下，即磁方位角代替地理方位角；磁场坐标系为m系，由n系到m系的姿态矩阵为：

又因为，前面已经利用加速度计得到了姿态角（pitch、roll）;

因此，三轴磁强计测量磁场为

利用Cnh和归一化地磁矢量在水平坐标系的投影，得到：

最需要注意的是hn的计算：

四、捷联式地平仪的工作原理

器件构成：陀螺仪和加速度计；

其中，利用陀螺仪的输出进行姿态更新（利用四元数）；利用加速度计的比力输出和速度误差方程构建失准角模型；

注意点：

以上的应用前提是速度平稳或低加速度环境下；因此，如何判断载体当前的运动状态尤为重要；

参考链接：《捷联惯导算法与组合导航原理讲义》