前言

IMU中的加速度计及陀螺仪测得原始数据为速度增量及角度增量，需要通过机械编排来将IMU原始输出转化为姿态、速度、位置信息，本文主要参考严恭敏老师的《捷联惯导算法与组合导航原理讲义》中捷联惯导更新的相关内容，整理了INS机械编排的相关算法公式，便于快速回顾复习。

姿态更新

姿态微分方程

方向余弦矩阵：

经过小角度近似，方向余弦矩阵可以简化为：

易得：

由方向余弦矩阵微分方程定义为：

其中：

代入得：

利用毕卡逼近法求解上述微分方程得：

假设积分周期内，b系定轴转动，有：


当R为惯性系i时，可得：

姿态更新算法

设导航系为n系，则有姿态更新方程：

假设陀螺在时间段[tk-1,tk]内（T = tk - tk-1）进行了两次等间隔采样，采用双子样圆锥误差补偿算法，有：

导航系n系中，短时间内由载体速度和位置引起的n系相对于惯性系的角速度变化很小，可视为常值（地球自转），则有：

至此，完成姿态更新。

速度更新

速度微分方程

导航坐标系n系下的惯导比力方程：

速度更新算法

速度微分方程积分得：

即：
上式右边依次为积分初值、比力积分项和重力/哥氏积分项。

重力/哥氏积分项

由于短时间内载体引起的导航坐标系旋转及重力矢量的变化都是很小的，一般认为重力/哥氏积分项的被积函数是时间的缓变量，可以采用中间时刻tk-1/2=(tk-1+tk)/2的值进行近似代替，即：

由于此时并不知道tk时刻的速度及位置等参数，上式中的tk-1/2时刻的各量需要用外推法计算，表示如下：

比力积分项

当等效旋转矢量为小量时，可取如下一阶近似：

代入得：

下面进一步讨论上式右端后两个积分的计算方法：

右端第三积分项

移项整理得：

在时间段tk-1到tk内，对上式两边同时积分得：

上式中，第一项成为速度的旋转误差补偿项，它因比力方向在空间旋转变换引起。

第二项成为划桨误差补偿项，划桨误差补偿项一般无法求得精确解，为了近似处理，假设陀螺仪角速度和加速度计比力测量均为线性模型，采用双子样速度划桨误差补偿算法可得：

右端第二积分项

右端第二积分项，其形式上与右端第三积分项完全相同。类似地，我们可以求解出右端第二积分项：

速度更新方程

至此，得到速度更新方程：

进一步近似，设：

得：

位置更新

位置微分方程

捷联惯导系统的位置(经度、纬度和高度)微分方程：

改写为矩阵形式为：

位置更新算法

相较于速度和姿态更新算法，位置更新算法引起的误差一般比较小，可采用梯形积分法对微分方程进行离散化（其中M矩阵采用线性外推算法求得）：

至此完成位置更新及INS机械编排。