两种建模方法：动力学建模（Dynamic Modeling）、运动学建模（Kinematic Modeling）

动力学建模： 对力与相互作用引起的运动进行描述。表现一个物理系统对另一个物理系统的影响。例如：牛顿第二运动定律，表达了由力模型：F{r, v, t} 引起的运动变化。其中r是位置向量，v是速度向量，这些被称为状态向量，时间t是一个自变量。
F{r, v, t} = ma= mv’= mr’’
r( t) =r( t, b, c) 其中c，b为常数
r( 0) =r( 0, b, c)
v( 0) =v( 0, b, c)

地球的自转和描述卫星绕地球运行的运动通常使用动力学建模。但不适用于物体在地球表面的运动。

运动学建模：运动学建模常用于对运动物体进行动力学建模不切实际的情况。它研究的是物体的运动，而不考虑其质量或作用于其上的力，从而排除了力模型。在运动学建模中，物体的运动完全是根据位置、速度和加速度测量值来确定的，这些测量值统称为可观测值。
根据可观测到的结果，位置矢量可表示为：r( t)={ x( t ) , y( t ) , z ( t ) }
速度矢量为：

加速度矢量为：

Mechanization：是把IMU的输出转换成位置、速度和姿态信息的过程。IMU输出包括陀螺仪测量的三轴的旋转速率和加速度计测量的三轴加速度。系统框图如下所示：

加速度计的输出称为 比力f（除了重力以外的力）：

其中，f为比力，a为载体测得的加速度，g为 重力矢量。
测量值是在载体系（b）获得的，通常假定传感器的三轴与载体系重合，此时测量值可以通过3x3的旋转矩阵Rib变换到惯性系上（从b系变换到i系）：

i系下的Mechanization方程可归结为：

ri =[ xi yi zi ] 是i系的三维位置矢量；vi =[ vxi vyi vzi ] 是i系的三维速度矢量
Wib为陀螺仪输出的角速度，旋转矩阵的导数=旋转矩阵*角速度的反对称矩阵

e系下的Mechanization方程可归结为：

其中输入为加速度计测量的速度和陀螺仪测量的转速。输出是位置向量r，速度向量v和欧拉角，都在e坐标系中表示。

local-level frame是最常用的坐标系，使用当地坐标系的优点：
a. 当地坐标系可以直观地表现地球表面用户的导航状态。
b. 由于I-frame的轴线与当地的东、北、天方向对齐，所以在当地坐标系中求解时，可以通过方程的输出处获得姿态角(俯仰、横滚和方位角)
c. 水平(E-N)平面上导航参数的计算误差受舒勒效应的约束。
d. 舒勒效应规定了水平面元件的惯性系统误差耦合产生舒勒回路，这些误差以舒勒频率的1/5,000 Hz振荡。

最常用的三种方法是欧拉角法、方向余弦法和四元数法。
欧拉角只需要三个独立的参数。方向余弦（旋转矩阵）有9个参数，其中6个是独立的。这两种方法在计算上较为复杂，不适用于实时的计算，并且欧拉角容易产生奇异点。旋转矩阵的求解是不封闭的。因此，参数化转换矩阵最有效的方法是使用四元数方法。

从b系到k系的旋转矩阵随时间的变化率表示为：
其中，omga是传感器测量角速率[wx,wy,wz]的反对称矩阵：

求旋转矩阵的导数 需要求解9个微分方程，才能从角速度数据中得到变换矩阵。
现在讨论这个方程的一个封闭解。假设角速度w在很小的时间间隔内是恒定的。则旋转角度的增量为：

根据推导式，可以将旋转矩阵写成递归的形式：

省略中间推导过程，最终得到以下结果：

欧拉定理指出,刚体相对于参考系的旋转，可以表示刚体绕固定轴旋转一个角度。

上图表示了一个角轴旋转，其中矢量n为旋转轴，theta为绕n旋转的角度。三个角 alpha、beta、gama定义了单位矢量旋转轴n的方向。
四元数可通过角轴表述为：

四元数由一个实部与三个虚部构成，根据上式可知，四元数通过三个独立的分量便足以描述刚体的旋转，并且存在以下约束：

四元数的导数为：


将其表示为递推式：

令：


则：

在旋转速率较低的情况下，可以采用数值积分的方法求解，即：


相当于对递推结果进行一阶近似。

若已知四元数 q=[ q1,q2,q3,q4],则可通过下式得到旋转矩阵：

同样，在确定旋转矩阵后，也可将其转化为四元数（对准过程中测量的姿态角确定初始旋转矩阵后，计算四元数的初始值）：

四元数方法的优点：
a. 四元数只需要求解4分方程（4个自由度），而旋转矩阵需要求解6个方程（6个自由度）。
b. 四元数避免了使用欧拉角时可能出现的奇异性问题。欧拉角奇异性问题
c. 四元数计算相对简单

四元数方法的缺点：
结果中存在非线性项以及需要在计算周期中重新正规化。

以下罗列使用欧拉角、旋转矩阵、四元数三种方法对旋转进行参数化的优点和缺点：

a.陀螺仪中获得的旋转速度(wx，wy ，wz)与加速度计测得的加速度（ fx，fy，fz）构成IMU输出。
b.根据加表与陀螺仪计算俯仰，横摇和方位(p,r, A)的姿态角
第四章中【通过加速度测量的比力，可以计算出初始姿态角中的pitch和roll，若角速率(wx，wy ，wz)可以感知地球自转则可以求得航向角】。由此得到b系相对于local系的旋转矩阵Rlb。
c.使用前一步计算得到的Rlb，将b系中的比力转化为local系中的比力，得到东北天坐标系下的加速度为：(fe, fn, fu)。
d.加速度计除了比力外，测量值中还包含地球引力与科氏力，因此需要除去这两个影响因素。
e.通过对东北天坐标系下的加速度 (fe, fn, fu)进行积分，计算东、北、天的速度(Ve, Vn, Vu)。
f.通过对东、北、天的速度(Ve, Vn, Vu)进行积分，计算 大地坐标系下的位置（纬度，经度，高度）。

INS的准确性受到各种因素的影响，如：初始对准过程中的误差、传感器误差、算法误差。
若知道这些误差对导航参数(位置、速度和姿态)的影响，就可以通过建模来进行误差的估计，从而减小误差对导航结果的影响。
因此，误差模型是分析和估计惯性导航系统误差源所必需的。估计器包括：卡尔曼滤波（KF) ；粒子滤波(PF) 和AI技术统上。
Mechanizaion状态方程用于确定性动态系统物理过程的描述。这些方程的解中含有误差(确定性和随机性)，因此需要使用传感器误差模型来进行分析和估计。
动态系统的的误差随时间变化，因此可以用微分方程来描述。由于这些方程是非线性的，在使用卡尔曼滤波之前需要将它们线性化。

根据上一章可知，导航参数方程具有以下关系：

LLF系（东北天）下的误差状态方程包括：纬度、经度、高度误差；东北天三个方向的速度误差、三个姿态角的误差；以及加速度计误差和陀螺仪漂移；
总误差为15维的状态矢量：

位置误差指的是，地理位置（纬度、经度、海拔）估计值的误差，已知位置的导数为：

位置误差定义为：

通过一阶泰勒展开，整理等式右侧，并省略矩阵中的极小项，得到位置误差的导数具有以下形式：


由上式可知，位置误差的状态方程与位置状态方程的转移关系近似相同。

加速度测量值需要去除科氏力和重力矢量的影响后，才可进行积分计算。

假定上式的误差可写成：

则，速度误差为：

将上式等号右边分为5个子项：

(1)根据反对称矩阵定理：Ab=-Ba，第一项可改写为：

其中：Fl为在L系上加速度比力矢量 fl:[ fe, fn, fu ] 的反对称矩阵；
(2)第二项

(3)第三项
由上一章已知，l系相对于e系的角速率、e系相对于i系的角速率为：

将角速率矢量转化为反对成矩阵形式，并代入到第三项中，得到：

(4)第四项
角速率的误差定义：

省略中间推导过程，可得到：

代入定义：

省略分母的二次方项，得到：

因此，第四项最后结果为：

(5)第五项
R为地球半径，g为重力矢量的模

经过整理合并，速度误差可写成：

方程右边的第三项和第四项中，包含以地球半径作为分母的元素，以及乘以地球自转角速度（we）的元素，这些元素相对较小，在实际计算可忽略不计。因此，省略较小项的速度误差可简洁的表示为：

注意：加速度分量fe 、fn与重力矢量相比是很小的(接近于零),而 fu 接近重力加速度(9.8米/ sec2)。
因此，根据上述关系可知 deta( ve)与deta( r)、deta( vn)与deta( p)之间存在强耦合关系，而deta(ve)、deta(vn)与deta(A)之前是一种弱耦合的关系。

姿态误差：两个坐标系相对旋转的误差，实际上是由角速度误差引起的。
根据上一章，已知旋转矩阵的状态方程：

由于理论计算值存在误差，因此与真实值之间存在一个误差小量：（用带上三角的符号表示理论计算值，不带上三角的符号为真实值）

其中，旋转误差小量可以写为【欧拉角误差的反对称矩阵】乘以【两个坐标系之间的真实旋转矩阵】的形式：

将包含误差的式子代入旋转矩阵的状态方程：

同时b系相对于l系的角速度的反对称矩阵也可写成：真实值角速度+线性角速度误差 的形式：

这样就得到了一个旋转矩阵的导数与角速率误差、姿态角误差的关系式，为了进一步探究姿态角误差的变化与说明因素有关，我们可以通另一个方式获得一个等价的方程。

为了得到矩阵R的变化率，需要对理论测量的旋转矩阵求导，用姿态角误差变化率表示变换矩阵R的变化率：


通过两个等价方程，可以得到：

消去等式两侧的相同元素：


等式右边第二项为二阶误差，可以忽略不计，因此可以得到姿态角误差的变化率为：

上式是反对称矩阵的形式，将其转化为矢量形式的姿态角误差变化率：

上式说明了：姿态误差=[ deta( p) ,deta( r) ,deta( A) ]T 可通过角速度误差deta(Wlb)表示。

下面分析角速度误差deta(Wlb)的表达式；
载体系b相对于大地系l的 角速度Wlb 由l系相对于惯性系i的角速率（Wil） - 传感器测量的b系相对于i系的角速率（Wib） 得到，即：

角速度误差deta(Wlb)可表示为：

根据反对称矩阵公式：Ab=-Ba ，有:

将deta(Wlb)代入角速度误差方程：


根据上式，可知姿态角误差由以下几个因素构成：1. 导航参数误差 deta(Wil) ； 2.载体系旋转速率的测量误差 deta(Wib)；3. 以及i系相对于l系的角速率的反对称矩阵

导航参数误差 deta(Wil)具有以下定义：

同样，i系相对于l系的角速率的反对称矩阵



综合以上式子，可以得到姿态角误差的变化率为：

其中，第三项和第四项中包含地球半径的倒数项、以及地球旋转的乘积项，在大多数的导航计算中可以将其忽略不计，因此姿态角误差可以仅保留前两项，简化为：
上式隐含的强耦关系也被称为舒勒效应。

传感器误差主要是陀螺仪漂移和加速度计偏差，包含确定性误差与非确定性误差。确定性部分在实验校准过程中进行计算，并在测量中进行补偿。传感器误差的不确定性部分是随机的，采用随机模型进行建模。
这些误差通常在时间上是相关的，常用的建模方法包括：随机游走过程、一阶高斯-马尔科夫（GM）过程和自回归（AR）过程。

传感器的随机误差模型一般采用一阶GM过程进行建模，其一般形式为：

x: 随机过程
beta：过程相关时间的倒数
W： 零均值不相关的高斯噪声的单位协方差
segma2：随机过程高斯白噪声的协方差

（1）加速度计Bias误差

（2）陀螺仪漂移drift误差

L系的位置、速度、姿态的状态误差以及传感器误差，可归结为：

其中

状态误差的流程框图：

若将上述误差状态方程写为一阶微分形式：

舒勒效应与速度误差deta(ve)与deta(vn)、俯仰角误差deta( p)与横滚角误差deta( r)在水平面上的耦合有关。这种耦合关系限制了水平和垂直速度以及俯仰和滚转角的误差。

根据误差模型，我们已知，横滚角roll误差的状态方程为：

东向的速度误差Ve的状态方程为：

因为 fu接近重力加速度g，而 fn 非常小，因次上式可进行简化，可知 deta（ve）与deta（r）之间存在强耦合的关系：

对上式求导（东向速度误差求二阶导），并代入roll误差的导数关系式，可得：

方程解得的 东向速度误差deta（ve）随时间振荡的频率非常小，等于1/5000 Hz。将这种小频率震荡称为 舒勒频率 fs，时间间隔为84.4分钟。因此，速度误差是有界的。
舒勒频率：

同理，若是对roll误差求二阶导，并代入东向速度误差的导数，得到：

方程解得到的横滚角误差deta（r）同样会随时间以舒勒频率振荡。因此，姿态误差会随着时间的推移而变得有界。

东向速度误差deta（ve）与横滚角误差deta（r）之间存在强耦合关系，也就是说，如果从外部(如GPS)对INS的东向速度进行更新，从而得到准确的deta（ve）估计，也会使得横滚角误差deta（r）变得更准确。换句话说，速度的更新使得东向速度误差deta（ve）可观，而由于强耦合关系，这种可观性也会延伸到横滚角误差deta（r）上。

与东向的误差相似，舒勒效应 同样也体现在北向的误差模型上。
根据姿态误差的状态方程，可知 俯仰角pitch的误差变化率为：

而与之存在紧耦合关系的是北向的速度误差deta（vn）：

简化为：

分别对俯仰角pitch的误差和北向速度误差做二次微分：


方程解得的俯仰角pitch的误差随时间以舒勒频率 fs 振荡；**北向的速度误差deta（vn）与俯仰角的误差deta（p）**之间的强耦合如图所示。

当系统的北向速度由外部源更新时，不仅北向的速度误差deta（vn）得到了准确估计，由于强耦合关系，俯仰角的误差deta（p）也得到了准确估计。

导航系统的精度受到 惯性传感器初始化以及算法的误差影响。低成本MEMS传感器由于严重的随机误差，INS输出可能迅速漂移。因此低精度的IMU基本上不能作为导航独立传感器进行使用。
在之前的章节中曾叙述过，对于水平面的速度与姿态角而言，若没有对速度进行持续的优化以消除速度误差，在长时间运动情况下会导致Vn和Ve值产生巨大的误差。同时，姿态角的误差，也需要依靠这些速度值来调节 ——【舒勒效应】。因此，建立精确的速度分量模型是十分重要的（可消除pitch、roll角的误差）。

几类典型的测量更新方式：
1）位置 或坐标更新——“CUPT”
2）速度 或零速度更新——“ZUPT”
3）姿态更新

通过外部测量进行INS更新的方法有很多种：包括卡尔曼滤波(KF)、粒子滤波(PF)和人工智能(AI)。
通常，将惯性导航系统与GPS辅助系统相结合，利用卡尔曼滤波对惯性传感器误差进行估计和补偿。总的来说，卡尔曼滤波器是一种从被噪声污染的测量值中最优估计系统状态误差的算法。这是一个顺序递归算法，提供了一个最佳的误差最小均方差估计。
卡尔曼滤波的优秀之处在于，它使用了所有可用的测量数据，不论它们的精度高低，都可以通过对测量值增加合适的权重来估计当前系统状态。

在惯性导航应用中，卡尔曼滤波用于互补配置，将具有不同噪声特性的同一信号的冗余测量值结合起来，最小化误差。惯导系统能提供良好的高频信息，但由于积分运算，惯导系统的误差随时间不断增大，使得加速度计和陀螺仪的偏置误差在输出端积累。惯性传感器的偏置误差通常出现在传感器输出的低频部分，称为长期误差。另一方面，许多其他导航系统(如GPS)具有良好的低频特性，但容易产生高频噪声。因此，利用KF可以从外部源的准确低频数据中获益，从而限制INS的长期误差。有许多外源可以为INS提供可靠的帮助，包括雷达、GPS、车轮传感器、激光测距和存储的图像数据库。
在下文中将主要考虑全球定位系统作为外部源（由于GPS的准确性，全球可用性和低成本）。

卡尔曼滤波要求系统为线性的系统，离散时间的线性系统可描述为：

其中：Wk-1为过程噪声。由于噪声对动态系统的影响表现在状态矢量的多个分量上，因此需要使用一个噪声分布矩阵G来表述噪声与各个状态分量之间的耦合性。

系统的离散时间线性测量方程为：

卡尔曼滤波基于以下假设：

1.状态方程和测量方程均可用线性模型表示；
2.系统噪声Wk以及测量噪声 为不相关的零均值高斯白噪声：


其中，Qk和Rk是正定矩阵。在INS/GPS积分中，
Qk表示与INS误差相关的系统噪声的协方差矩阵，
Rk表示与GPS位置和速度更新相关的测量噪声协方差矩阵。

3.系统初始状态向量xo是一个与过程和测量噪声都不相关的随机向量，因此：

4.初始状态x0以及其协方差矩阵P0是已知的：

KF是一种递归算法，通过状态反馈来估计系统的状态。
如图所示，卡尔曼滤波分为两个环节:(1)预测 /状态更新; (2)校正 /测量更新。
在预测阶段，将系统状态及其协方差从k -1时刻传播到k时刻；然后在校正阶段，使用测量值更新先前的估计，最终得到校正后的状态估计。

根据 k-1 时刻的信息估计 k 时刻的系统状态，称之为“先验估计”——Xk(-)，假定系统噪声为零均值，则 k 时刻的状态估计可写为：

其中Xk-1(+)为上一时刻的最优估计。
同时，卡尔曼滤波还将 上一时刻 k-1 的不确定度传递到下一时刻 k，不确定度通过误差协方差来表示 Pk(-)，也称为先验协方差矩阵：

其中 Pk-1(+)为上一时刻的协方差矩阵的最优估计，由 Pk(-) 和过程噪声Qk得到。

在预测了状态的先验估计之后，当获得外部源的测量信息时，先验估计会得到更正。
测量协方差为Rk，表示测量数据的不确定度。
在测量更新环节中，最重要的一步为：计算卡尔曼增益的加权因子 K

卡尔曼增益使KF在最优估计算法中脱颖而出。
由方程可知，K既依赖于先验协方差Pk(-)，又依赖于测量噪声协方差Rk。如果测量噪声较大(Rk较大)，或者过程噪声较小(Pk(-)较小）时，那么K就会变得相对较小。反之，若过程噪声较大(Pk(-)加大)或测量值的噪声较小时(Rk 较小)，则K相对较大。
当K较大时，测量值将被赋予更多的权重，当K较小时，预测值的权重就相对较大。

当 tk 时刻得到一个新的测量值 Zk 时，将其与基于先验状态估计的预测测量值 HkXk(-) 进行比较。用K对二者误差进行加权，并更新状态向量的预测以生成最佳估计。因此，tk 时刻的状态估计值为：

在更新完系统状态之后，还需对新估计值 Xk(+) 的不确定度进行更新：

对于上式的协方差矩阵更新公式P. D. Joseph 认为，即便求得的卡尔曼增益K 具有细小误差，但传递到上式时依旧会产生能严重的问题。因此，他主张使用一种被称为约瑟夫式的方程的扩展形式：

上式得到的后验协方差在数值上是稳定的，即使K的计算有误差也能得到正确的答案。此外，这种形式的后验误差协方差可以保证Pk(+)是半正定的，有助于避免发散。

在大多数INS应用中，KF更新过程的频率低于预测。例如，通过卡尔曼滤波将GPS和INS集成的典型应用中，预测可能以100hz的频率进行，而更新以1hz的频率进行。下图显示了典型组合系统的预测和更的过程：

离散KF算法的数据流图：

时间更新与测量更新的方程归结如下：

KF由5个步骤组成，算法流程如图所示：

具体步骤如下：

1. 首先，初始化滤波器。这需要为过滤器提供其状态初始估计X0和初始估计中的不确定性P0。Po的估计是基于对初始状态估计的近似精度，通常设置为相对较高的值。同时还需要向滤波器提供系统噪声协方差矩阵Q和测量噪声协方差矩阵R的初始估计值。这些估计值是根据之前的系统经验估算出来的。
2. 预测步骤的第一部分：计算状态转移矩阵，将初始状态从 k-1时刻传递到k时刻, 并表示为Xk(-).
3. 预测步骤的第二部分：计算先验状态的协方差Pk。通过状态转移矩阵、上一时刻的协方差Pk-1、过程噪声协方差Qk-1，以及噪声分布矩阵Gk-1得到。需要注意的是，如果过程噪声较大，Pk就会增大，从而导致先验状态的置信度降低。
4. 测量更新阶段的第一步：计算卡尔曼增益Kk。卡尔曼增益的值取决于先验误差协方差Pk(-)、过程噪声协方差Rk、观测方程中的Hk。Pk(-)越大，增益越大，Rk越大，增益越小。
5. 测量更新阶段的第二步： 继完成预测的第二部分后，当接收到测量值时，先验状态k(-)就会被更新。这一步基于预测的测量值HkXk(-)与实际测量值的差值。根据这一差值进行状态估计的修正，当K较大时，这个差值的权重更大，并添加到先验估计值中，并更新为后验估计值Xk(+)。但当K较小时，从测量中获得的校正信息的权重较小，此时认为先验估计相对准确。
6. 测量更新阶段的第三步：在修正状态估计之后，KF进一步将先验误差协方差Pk(-)更新为后验误差协方差Pk(+)，以表示后验估计的不确定度。
7. 卡尔曼滤波进入下一时刻的状态估计与测量更新。

线性系统是理想化的结果，严格地说，几乎所有的系统都是非线性的（甚至欧姆定律也只是在有限范围内的一个近似，它的线性在超过一定的电压阈值之后就会失效(Simon 2006)）。
由于许多系统非常接近线性，通过对非线性系统进行线性化，可以将线性估计理论应用于非线性系统。在非线性的卡尔曼滤波中，通常将误差状态作为估计值，而不是以系统状态作为状态估计。在GPS/INS的组合导航中，误差状态是由INS状态和辅助源状态(GPS)之间的差值形成的。可用的方法有线性KF (LKF)和扩展卡尔曼滤波（EKF）。

对于某一个系统，在标准轨迹附近进行线性化的的方法，被称为线性化卡尔曼滤波（LKF）。此处的标准轨迹通常是已知的，例如，船或客机的航线是预先规划的。对于INS/GPS组合导航系统来说，惯导的输出通常被认为是标准轨迹。
线性化卡尔曼滤波是一个开环结构，其滤波器估计的误差虽然对INS的输出进行校正，但是对INS系统却不构成反馈。

轨迹的真值是无法事先知道的，因此通常将轨迹最优估计的结果作为标准轨迹。
当卡尔曼滤波使用先前最优估计的线性化轨迹，