[TOC]
捷联惯更新是基于上一时刻的PVA，PVA方差、零偏比例、角速度随机游走、加速度随机游走，通过 IMU 量测值（增量形式的比力、角速度），计算得到当前时刻的PVA、PVA方差、零偏比例、零偏比例方差阵。
- 捷联惯导不存在平台惯导中的物理平台，需要实时计算出数学平台；姿态更新就是建立起数学平台过程，所以最重要且对精度影响最大。
- 捷联惯导输出的角增量、比力增量都是在 i 系下的，我们关心的姿态都是在 n 系下的；捷联惯导中的 n 系不固定，需要先确定出 n 系相对于 i 系的关系
$C_{i n}^n$ 。 - 载体的姿态指的是载体坐标系（b系）和导航坐标系（n系）之间的方位关系
$C_n^b$ 。 - 导航系（n系）与地心地固系（e系）直接的方位关系
$C_n^e$ 与经纬度有关，称为位置矩阵，可由经纬度计算得到，也可以用于计算经纬度。 - 受到地球自转、重力加速度、地球曲率引起的牵连角速度等影响，姿态、速度无法通过直接积分获取，需要扣除它们对角速度、线速度的影响。
- 四元数、方向余弦阵微分方程的离散化数值求解都基于定轴转动假设，会带来不可交换性误差；通过单字样+前一周期的算法，补偿姿态更新中的圆锥运动、速度更新中的划桨效应。
- 递推计算式中有很多的积分，其中有些项的积分变量变化较慢，可以简单用梯形积分来算，也就是用两时刻的矩阵乘以时间间隔。
-
计算 sin(纬度)、cos(纬度)、tan(纬度)、sin(纬度)平方，方便后面使用。
-
计算计算子午圈半径 RM、卯酉圈半径 RN： $$ R_{M}=\frac{R_{e}\left(1-e^{2}\right)}{\left(1-e^{2} \sin ^{2} L\right)^{3 / 2}}、R_{N}=\frac{R_{e}}{\sqrt{1-e^{2} \sin ^{2} L}} $$
-
计算地球自转角速度造成的 n 系旋转： $$ \boldsymbol{\omega}{i e}^{n}=\left[\begin{array}{lll}0 & \omega{i e} \cos L & \omega_{i e} \sin L\end{array}\right]^{\mathrm{T}} $$
-
计算牵连角速度引起的 n 系旋转： $$ \boldsymbol{\omega}{e n}^{n}=\left[\begin{array}{lll}-\frac{v{\mathrm{N}}}{R_{M}+h} & \frac{v_{\mathrm{E}}}{R_{N}+h} \quad \frac{v_{\mathrm{E}}}{R_{N}+h} \tan L\end{array}\right]^{\mathrm{T}} $$
-
确定出 n 系相对于 i 系（惯性系，固定）的姿态： $$ \boldsymbol{\omega}{i n}^{n}=\boldsymbol{\omega}{i e}^{n}+\boldsymbol{\omega}_{e n}^{n} $$
-
根据纬度和高程，计算重力加速度： $$ g_{L h}=g_{0}\left(1+\beta \sin ^{2} L-\beta_{1} \sin ^{2} 2 L\right)-\beta_{2} h $$
-
计算重力加速度在 n 系投影： $$ \boldsymbol{g}^{n}=\left[\begin{array}{lll}0 & 0 & -g\end{array}\right]^{\mathrm{T}} $$
-
计算有害加速度积分项： $$ -\left(2 \boldsymbol{\omega}{i e}^{n}+\boldsymbol{\omega}{e n}^{n}\right) \times \boldsymbol{v}_{e n}^{n}+\boldsymbol{g}^{n} $$
速度更新主要有两部分要算：
- 比力积分项：其中需要补偿划桨效应，是快变量，需要仔细积分。
- 有害加速度积分项：包括重力、哥氏加速度、向心力，是慢变量，直接梯形积分，用中间时刻来算。
然后，当前时刻的速度 = 上一时刻速度 + 比力积分项 + 有害加速度积分项： $$ \begin{aligned} \boldsymbol{v}{e b, k}^{n}=\boldsymbol{v}{e b, k-1}^{n} & +\underbrace{\left[\boldsymbol{I}-\frac{1}{2}\left(\boldsymbol{\zeta}{n(k-1) n(k)} \times\right)\right] \boldsymbol{C}{b(k-1)}^{n(k-1)} \Delta \boldsymbol{v}{f, k}^{b(k-1)}}{n \text { 系比力积分项 }} \ & +\underbrace{\boldsymbol{g}{l}^{n} \Delta t{k}-\left(2 \boldsymbol{\omega}{i}^{n}+\boldsymbol{\omega}{e n}^{n}\right) \times\left.\boldsymbol{v}{e b}^{n}\right|{t=t_{k-1 / 2}} \Delta t_{k}}_{\text {重力/哥氏积分项 }}\end{aligned} $$
由 n 系速度更新纬经高的方程为： $$ \dot{L}=\frac{1}{R_{M}+h} v_{\mathrm{N}}, \quad \dot{\lambda}=\frac{\sec L}{R_{N}+h} v_{\mathrm{E}}, \quad \dot{h}=v_{\mathrm{U}} $$ 写成矩阵形式为： $$ \dot{p}=M_{p v} v^{n} $$ 其中位置更新矩阵 $\boldsymbol{M}{p v}$： $$ \boldsymbol{M}{p v}=\left[\begin{array}{ccc}0 & 1 / R_{M h} & 0 \ \sec L / R_{N h} & 0 & 0 \ 0 & 0 & 1\end{array}\right] $$ 位置更新误差比较小，所以可以采用梯形积分，也就是用两时刻速度的均值更新位置，先计算两时刻位置的增量： $$ \boldsymbol{p}{m}-\boldsymbol{p}{m-1}=\int_{t_{m-1}}^{t_{m}} \boldsymbol{M}{p v} \boldsymbol{v}^{n} \mathrm{~d} t \approx \boldsymbol{M}{p v(m-1 / 2)}\left(\boldsymbol{v}{m-1}^{n(m-1)}+\boldsymbol{v}{m}^{n(m)}\right) \frac{T}{2} $$ 先前速度 + 两时刻位置增量，得到当前速度： $$ \boldsymbol{p}{m}=\boldsymbol{p}{m-1}+\boldsymbol{M}{p v(m-1 / 2)}\left(\boldsymbol{v}{m-1}^{n(m-1)}+\boldsymbol{v}{m}^{n(m)}\right) \frac{T}{2} $$ 综上，速度更新就是先算 $\boldsymbol{M}{p v}$，然后算两时刻位置增量，加到先前位置上得到当前位置。
姿态更新主要也算两部分：
- 两时刻 n 系的变化：由地球自转角速度、牵连角速度两部分引起。
- 两时刻 b 系的变化：通过 IMU 角增量量测值计算，需补偿圆锥运动。
然后，两时刻 n 系的旋转四元数 * 上一时刻姿态四元数 * 两时刻 b 系旋转四元数得到当前姿态： $$ \boldsymbol{q}{b{k}}^{n_{k}}=\boldsymbol{q}{n{k-1}}^{n_{k}} \boldsymbol{q}{b{k-1}}^{n_{k-1}} \boldsymbol{q}{b{k}}^{b_{k-1}} $$
连续时间系统状态方程： $$ \dot{\boldsymbol{x}}(t)=\boldsymbol{F}(t) \boldsymbol{x}(t)+\boldsymbol{G}(t) \boldsymbol{w}(t) $$ 其中，噪声状态转移矩阵 F： $$ \boldsymbol{F}(t)=\left[\begin{array}{lllll}\boldsymbol{F}{a a} & \boldsymbol{F}{a v} & \boldsymbol{F}{a p} & -\mathbf{C}{b}^{n} & \boldsymbol{0}{3 \times 3} \ \boldsymbol{F}{v a} & \boldsymbol{F}{v v} & \boldsymbol{F}{v p} & \mathbf{0}{3 \times 3} & \mathbf{C}{b}^{n} \ \mathbf{0}{3 \times 3} & \boldsymbol{F}{p v} & \boldsymbol{F}{p p} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} \ \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \boldsymbol{F}{\varepsilon} & \mathbf{0}{3 \times 3} \ \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \boldsymbol{F}_{\nabla}\end{array}\right] $$
$$ \begin{array}{l}\boldsymbol{M}{1}=\left[\begin{array}{ccc}0 & 0 & 0 \ -\omega{i e} \sin L & 0 & 0 \ \omega_{i e} \cos L & 0 & 0\end{array}\right] \boldsymbol{M}{2}=\left[\begin{array}{ccc}0 & 0 & v{N} / R_{M h}^{2} \ 0 & 0 & -v_{\mathrm{E}} / R_{N h}^{2} \ v_{\mathrm{E}} \sec ^{2} L / R_{N h} & 0 & -v_{\mathrm{E}} \tan L / R_{N h}^{2}\end{array}\right] \ \boldsymbol{M}{3}=\left[\begin{array}{ccc}0 & 0 \ -2 \beta{3} h \cos 2 L & 0 & -\beta_{3} \sin 2 L \ -g_{0}\left(\beta-4 \beta_{1} \cos 2 L\right) \sin 2 L & 0 & \beta_{2}\end{array}\right]\end{array} $$
姿态：
$$
\left.\boldsymbol{F}{a v}=\left[\begin{array}{ccc}0 & -1 / R{M h} & 0 \ 1 / R_{N h} & 0 & 0 \ \tan L / R_{N h} & 0 & 0\end{array}\right] \quad \begin{array}{l}\boldsymbol{F}{a a}=-\left(\boldsymbol{\omega}{i n}^{n} \times\right) \ \boldsymbol{F}{a p}=\boldsymbol{M}{1}+\boldsymbol{M}{2}\end{array}\right}
$$
速度：
$$
\left.\begin{array}{l}\boldsymbol{M}{v a}=\left(\boldsymbol{f}{\mathrm{sf}}^{n} \times\right) \ \boldsymbol{M}{v v}=\left(\boldsymbol{v}^{n} \times\right) \boldsymbol{M}{a v}-\left[\left(2 \boldsymbol{\omega}{i e}^{n}+\boldsymbol{\omega}{e n}^{n}\right) \times\right] \ \boldsymbol{M}{v p}=\left(\boldsymbol{v}^{n} \times\right)\left(2 \boldsymbol{M}{1}+\boldsymbol{M}{2}\right)+\boldsymbol{M}{3}\end{array}\right}
$$
位置：
$$
\boldsymbol{F}{p v}=\left[\begin{array}{ccc}0 & \frac{1}{R_{M}+h} & 0 \ \frac{\sec L}{R_{M}+h} & 0 & 0 \ 0 & 0 & 1\end{array}\right] \quad \boldsymbol{F}{p p}=\left[\begin{array}{ccc}0 & 0 & -\frac{v{N}}{\left(R_{M}+h\right)^{2}} \ \frac{v_{E} \sec L \tan L}{R_{M}+h} & 0 & -\frac{v_{E} \sec L}{\left(R_{N}+h\right)^{2}} \ 0 & 0 & 0\end{array}\right]
$$
比例零偏：
$$
\boldsymbol{F}{\varepsilon}=\left[\begin{array}{ccc}-\frac{1}{\tau{\varepsilon}} & 0 & 0 \ 0 & -\frac{1}{\tau_{\varepsilon}} & 0 \ 0 & 0 & -\frac{1}{\tau_{\varepsilon}}\end{array}\right], \quad \boldsymbol{F}{\nabla}=\left[\begin{array}{ccc}-\frac{1}{\tau{\nabla}} & 0 & 0 \ 0 & -\frac{1}{\tau_{\nabla}} & 0 \ 0 & 0 & -\frac{1}{\tau_{\nabla}}\end{array}\right]
$$
噪声驱动阵阵 G：
$$
\boldsymbol{G}(t)=\left[\begin{array}{llll}-\mathbf{C}{b}^{n} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} \ \mathbf{0}{3 \times 3} & \mathbf{C}{b}^{n} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} \ \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} \ \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{I}{3 \times 3} & \mathbf{0}{3 \times 3} \ \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{0}{3 \times 3} & \mathbf{I}{3 \times 3}\end{array}\right]
$$
噪声阵
$$ \boldsymbol{\Gamma}{k \mid k-1} \approx\left[\mathbf{I}+\frac{1}{2} \boldsymbol{F}\left(t{k-1}\right) \mathrm{T}{s}\right] \boldsymbol{G}\left(t{k-1}\right) \approx \boldsymbol{G}\left(t_{k-1}\right) $$
然后，