【多传感器融合定位SLAM专栏】前端里程计、IMU预积分、滤波、图优化推导与应用（5）

本专栏基于深蓝学院《多传感器融合定位》课程基础上进行拓展，对多传感器融合SLAM的学习过程进行记录

第五章 基于图优化的建图方法

基于预积分的优化流程

优化问题分析

滤波与图优化是融合的两种方式，不改变原始信息的性质。优化问题可以等效为如下形式
min ⁡ x { Σ ∣ ∣ r L ( L k k + 1 , X ) ∣ ∣ 2 （激光里程计约束） + Σ ∣ ∣ r B ( B k k + 1 , X ) ∣ ∣ 2 （ I M U 约束） + Σ ∣ ∣ r G ( G k , X ) ∣ ∣ 2 （ R T K 约束） } \min_x \{ \Sigma||r_L(L_k^{k+1},X)||^2（激光里程计约束） + \Sigma||r_B(B_k^{k+1},X)||^2（IMU约束） + \Sigma||r_G(G_k,X)||^2（RTK约束）\} xmin​{Σ∣∣rL​(Lkk+1​,X)∣∣2（激光里程计约束）+Σ∣∣rB​(Bkk+1​,X)∣∣2（IMU约束）+Σ∣∣rG​(Gk​,X)∣∣2（RTK约束）}
三种约束分别通过以下方式获得：

1. 激光里程计约束：使用激光里程计，计算每个关键帧的位姿，进而得到相对位姿
2. IMU约束：在上一个关键帧位姿基础上，进行惯性积分，从而得到两关键帧的相对位姿
3. RTK约束：直接测量得到

预积分的作用

问题：位姿每次优化后会发生变化，其后的IMU惯性积分就要重新进行，运算量过大

解决思路：直接计算两帧之间的相对位姿，而不依赖初始值影响，即所谓的预积分

作用：只提高效率，不提高精度；利用IMU的值形成帧间约束

预积分模型设计

预积分的均值

连续时间的IMU预积分

已知导航的微分方程如下（推导见第三章三维运动微分性质）
$$\begin{gathered} {\dot{p}}_{w{b}_t}=v_t^w \\ {\dot{v}}_t^w=a_t^w \\ {\dot{q}}_{w{b}_t}=q_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\omega }^{b_t}\end{array}\right] \end{gathered}$$
根据该微分方程，可知从i时刻到j时刻的IMU积分结果为
$$\begin{array}{rl}& 位移p_{w{b}_j}=p_{w{b}_i}+v_i^w\Delta t+\int {\int }_{t\in [i,j]}(q_{w{b}_t}{a}^{b_t}-g^w)\delta t^2\\ & 速度v_j^w=v_i^w+{\int }_{t\in [i,j]}(q_{w{b}_t}{a}^{b_t}-g^w)\delta t\\ & 姿态q_{w{b}_j}={\int }_{t\in [i,j]}{q}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\omega }^{b_t}\end{array}\right]\delta t\end{array}$$
其中
$$\begin{gathered} a^{b_t}(真实值)={\hat{a}}_t(观测值)-b_{a_t}(零偏)-n_a(随机游走) \\ {w}^{b_t}={\hat{w}}_t-b_{w_t}-n_w \end{gathered}$$
可以发现每个积分都依赖着上个时刻的全局位姿状态，如$q_{w{b}_t}$。根据预积分的要求，需要求相对结果，而且不依赖于上一时刻的位姿状态，因此需要对上式进行转换。由于$q_{w{b}_t}=q_{w{b}_i}\otimes q_{b_i{b}_t}$，将其带入上式可得预积分式：重要
$$\begin{array}{rl}& p_{w{b}_j}=p_{w{b}_i}+v_i^w\Delta t-\frac{1}{2}{g}^w\Delta t^2+q_{w{b}_i}\otimes \int {\int }_{t\in [i,j]}(q_{b_i{b}_t}{a}^{b_t})\delta t^2=p_{w{b}_i}+v_i^w\Delta t-\frac{1}{2}{g}^w\Delta t^2+q_{w{b}_i}\otimes {\alpha }_{b_i{b}_j}\\ & v_j^w=v_i^w-g^w\Delta t+q_{w{b}_i}\otimes {\int }_{i\in [i,j]}(q_{b_i{b}_t}{a}^{b_t})\delta t=v_i^w-g^w\Delta t+q_{w{b}_i}\otimes {\beta }_{b_i{b}_j}\\ & q_{w{b}_j}=q_{w{b}_i}\otimes {\int }_{t\in [i,j]}{q}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\omega }^{b_t}\end{array}\right]\delta t=q_{w{b}_i}{\int }_{t\in [i,j]}\frac{1}{2}\Omega ({\vec{\omega }}^{b_t})q_{b_i{b}_t}\delta t=q_{w{b}_i}\otimes {\gamma }_{b_i{b}_j}\end{array}$$
此时积分项就完全和i时刻的位姿状态无关了，即积分不会随着第i时刻的状态改变而改变（积分中的是相对量不会随着全局位姿的优化而改变）。再次整理公式，把积分项用符号做标记有
$$\begin{gathered} {\alpha }_{b_i{b}_j}=\int {\int }_{t\in [i,j]}(q_{b_i{b}_t}{a}^{b_t})\delta t^2 \\ {\beta }_{b_i{b}_j}={\int }_{i\in [i,j]}(q_{b_i{b}_t}{a}^{b_t})\delta t \\ {\gamma }_{b_i{b}_j}={\int }_{t\in [i,j]}{q}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\omega }^{b_t}\end{array}\right]\delta t \end{gathered}$$

离散时间的IMU预积分递推式

实际使用中使用离散形式，而不是连续形式，在解算中一般采用中值积分方法，即
$$\begin{gathered} {\alpha }_{b_i{b}_{k+1}}={\alpha }_{b_i{b}_k}+{\beta }_{b_i{b}_k}\delta t+\frac{1}{2}a\delta t^2 \\ {\beta }_{b_i{b}_{k+1}}={\beta }_{b_i{b}_k}+a\delta t \\ {\gamma }_{b_i{b}_{k+1}}=q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\omega }^{b_t}\end{array}\right] \\ \omega =\frac{1}{2}[({\omega }^{b_k}-b_k^g)+({\omega }^{b_{k+1}}-b_k^g)] \\ a=\frac{1}{2}[q_{b_i{b}_k}(a^{b_k}-b_k^a)+q_{b_i{b}_{k+1}}(a^{b_{k+1}}-b_k^a)] \end{gathered}$$
经过以上推导，此时状态更新的公式可以整理为
$$\left[\begin{array}{c}{p}_{w{b}_j}\\ v_j^w\\ q_{w{b}_j}\\ b_j^a\\ b_j^g\end{array}\right]=\left[\begin{array}{c}{p}_{w{b}_i}+v_i^w\Delta t-\frac{1}{2}{g}^w\Delta t^2+q_{w{b}_i}{\alpha }_{b_i{b}_j}\\ v_i^w-g^w\Delta t+q_{w{b}_i}{\beta }_{b_i{b}_j}\\ q_{w{b}_i}\otimes {\gamma }_{b_i{b}_j}\\ b_i^a\\ b_i^g\end{array}\right]$$

零偏Bias的递推式

需要注意的是，陀螺仪和加速度计的模型为
$$\begin{gathered} b_{k+1}^a=b_k^a+n_{b_k^a}\delta t \\ {b}_{k+1}^g=b_k^g+n_{b_k^g}\delta t \end{gathered}$$
即认为bias是在变化的，这样有助于估计不同时刻的bias值，而不是整个系统运行时间内都当做常值对待。这更符合低精度mems的实际情况。但是在预积分时，有一两次相邻采样之间的时间较短，因此认为i和j时刻的bias相等。

需要注意的是，预积分的结果中包含了bias。在优化过程中，bias作为状态量也会发生变化，从而引起预积分结果变化。为了避免bias变化后重新做预积分，可以把预积分结果在bias处进行泰勒展开，表达成下面的形式。这样就可以根据bias的变化量直接算出新的预积分结果。
$$\begin{gathered} {\alpha }_{b_i{b}_j}={\bar{\alpha }}_{b_i{b}_j}+J_{b_i^a}^a\delta b_i^a+J_{b_i^g}^a\delta b_i^g \\ {\beta }_{b_i{b}_j}={\bar{\beta }}_{b_i{b}_j}+J_{b_i^a}^{\beta }\delta b_i^a+J_{b_i^g}^{\beta }\delta b_i^g \\ {q}_{b_i{b}_j}={\bar{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{J}_{b_i^g}^q\delta b_i^g\end{array}\right] \end{gathered}$$
其中
$$\begin{gathered} J_{b_i^a}^a=\frac{\partial a_{b_i{b}_j}}{\partial \delta b_i^a} \\ {J}_{b_i^g}^a=\frac{\partial a_{b_i{b}_j}}{\partial \delta b_i^g} \\ {J}_{b_i^a}^{\beta }=\frac{\partial {\beta }_{b_i{b}_j}}{\partial \delta b_i^a} \\ {J}_{b_i^{\beta }}^g=\frac{\partial {\beta }_{b_i{b}_j}}{\partial \delta b_i^g} \\ {J}_{b_i^g}^q=\frac{\partial q_{b_i{b}_j}}{\partial \delta b_i^g} \end{gathered}$$

在预积分的协方差部分将得到$\delta x_{k+1}=F\cdot \delta x_k+G\cdot n$ 形式的式子，对上式改写为
$$\begin{gathered} x_{k+1}=f(x_k) \\ {x}_{k+1}+\delta x_{k+1}=f(x_k+\delta x_k) \\ {x}_{k+1}+\delta x_{k+1}=f(x_k)+J\delta x_k \\ \delta x_{k+1}=J_k\delta x_k \end{gathered}$$
事实上雅可比矩阵是$x_k$对自身$x$ 的导数，即
$$J_k=\frac{\partial x_k}{\partial x}，J_{k+1}=\frac{\partial x_{k+1}}{\partial x}$$
上式和$\delta x_{k+1}=F\cdot \delta x_k+G\cdot n$ 合并同时忽略噪声项（与雅克比无关）将得到
$$F=\frac{\partial x_{k+1}}{\partial x_k}$$
因此，可以得到
$$J_{k+1}=F{J}_k$$
即可以通过k时刻的雅可比矩阵推导出k+1时刻的雅可比矩阵。算出来的雅可比矩阵是最终的预积分量对自身的雅可比矩阵。

预积分的协方差

1. 误差卡尔曼滤波：预测值与观测值都是误差，协方差算的是误差的协方差。误差状态帮助IMU预积分计算协方差时避免四元数的过参数化问题以及旋转向量的周期性问题。

   四元数是过参数化的，需要用4x4的矩阵来描述置信度；在误差卡尔曼滤波中，用旋转向量作为参数，这样就可以用3x3的协方差矩阵来衡量旋转的置信度；误差通常是很小的，接近0，这样就避免了旋转向量的周期性问题；误差量很小，可以忽略二阶导数的计算，只计算一阶导数更快；误差变化很慢，用卡尔曼滤波进行状态修正时，可以降低观测频率。

2. 为什么要计算误差状态传递方程

   我们想知道上一帧的误差是怎么传递到下一帧的，即想得到误差状态传递方程
   $${\delta }_{ik+1}=F_k{\delta }_{ik}+G_k{n}_k$$
   其中，状态量误差为${\delta }_{ik}=[\delta p_{ik},\delta v_{ik},\delta {\theta }_{ik}]$，测量噪声为$n_k=[n_k^a,n_k^g]$。

   如果已知上面的误差状态传递方程，那么一段时间内的IMU预积分的协方差就可以计算为
   $$P_{i,k+1}=F_k{P}_{i,k}{F}_k^T+G_k{Q}_n{G}_k^T$$
   其中，$Q_n$是测量噪声的协方差矩阵。递推从i时刻开始，$P_{i,k}=0$

   因此需要计算误差状态传递方程。如果能够推导状态误差随时间变化的导数关系，如
   $$\delta \dot{x}=A\delta x+Bn$$
   则误差状态的传递方程为
   $$\delta x_{k+1}\approx \delta x_k+\delta {\dot{x}}_k\Delta t=(I+A\Delta t)\delta x_k+B\Delta t{n}_k$$
   因此先计算$\delta x$ 的导数。

连续时间的IMU误差状态传递

• 姿态误差方程

（1）写出不考虑误差的微分方程
$${\dot{q}}_t=q_t\otimes \frac{1}{2}\left[\begin{array}{c}0\\ {\omega }_t-b_{{\tilde{w}}_t}\end{array}\right]$$
（2）写出考虑误差的微分方程
$$\dot{{\tilde{q}}_t}={\tilde{q}}_t\otimes \frac{1}{2}\left[\begin{array}{c}0\\ {\tilde{\omega }}_t-{\tilde{b}}_{w_t}\end{array}\right]$$
（3）写出带误差的值与理想值之间的关系
$$\begin{gathered} {\tilde{q}}_t=q_t\otimes \delta q \\ {\tilde{\omega }}_t={\omega }_t-n_w \\ {\tilde{b}}_{w_t}=b_{w_t}+\delta b_{w_t} \end{gathered}$$
其中$n_w$为陀螺仪白噪声，
$$\delta q=\left[\begin{array}{c}cos(|\frac{\delta \theta }{2}|)\\ \frac{\delta \theta }{|\delta \theta |}sin(\frac{|\delta \theta |}{2})\end{array}\right]\approx \left[\begin{array}{c}1\\ \frac{\delta \theta }{2}\end{array}\right]$$
$\delta \theta$是姿态误差对应的旋转矢量（失准角）。

（4）将（3）中的关系带入（2）
$$(q_t\otimes \delta q)=\frac{1}{2}{q}_t\otimes \delta \theta \otimes \left[\begin{array}{c}0\\ {\hat{\omega }}_t-n_{\omega }-b_{{\omega }_t}-\delta b_{{\omega }_t}\end{array}\right]$$
（5）将（1）中的关系带入（4）并化简得
$$\delta \dot{\theta }=-[{\hat{\omega }}_t-b_{{\omega }_t}{]}_{\times }\delta \theta -n_{\omega }-\delta b_{\omega t}$$

• 速度误差方程

（1）不考虑误差时的微分方程
$${\dot{v}}_t=R_t(a_t-b_{a_t})$$
（2）考虑误差时的微分方程
$$\dot{{\tilde{v}}_t}={\tilde{R}}_t({\tilde{a}}_t-{\tilde{b}}_{a_t})$$
（3）真实值与理想值之间的关系
$$\begin{gathered} \tilde{v}=v+\delta v \\ {\tilde{R}}_t=R_{t}exp([\delta \theta {]}_{\times })\approx R_t(I+[\delta \theta {]}_{\times }) \\ {\tilde{a}}_t=a_t-n_a \\ {\tilde{b}}_{a_t}=b_{a_t}+\delta b_{a_t} \end{gathered}$$
其中$n_a$为加速度计白噪声。

（4）将（3）的关系带入（2）
$$\dot{v}+\delta \dot{v}=R_t(I+[\delta \theta {]}_{\times })(a_t-n_a-b_{a_t}-\delta b_{a_t})$$
（5）将（1）的关系带入（4）
$$R_t(a_t-b_{a_t})+\delta \dot{v}=R_t(I+[\delta \theta {]}_{\times })(a_t-n_a-b_{a_t}-\delta b_{a_t})$$
（6）化简方程，忽略二阶小量
$$\delta \dot{v}=-R_t[{\hat{a}}_t-b_{a_t}{]}_{\times }\delta \theta -R_t(n_a-\delta b_{a_t})$$

• 位置误差方程

（1）不考虑误差时的微分方程
$$\dot{p}=v$$
（2）考虑误差时的微分方程
$$\dot{\tilde{p}}=\tilde{v}$$
（3）真实值与理想值之间的关系
$$\begin{gathered} \tilde{v}=v+\delta v \\ \tilde{p}=p+\delta p \end{gathered}$$
（4）将（3）的关系带入（2）
$$\dot{p}+\delta \dot{p}=v+\delta v$$
（5）将（1）的关系带入（4）
$$v+\delta \dot{p}=v+\delta v$$
（6）化简方程
$$\delta \dot{p}=\delta v$$

• bias误差方程

在IMU精度较高时，bias可以认为是常值，即
$$\begin{gathered} \delta {\dot{b}}_{a_t}=0 \\ \delta {\dot{b}}_{{\omega }_t}=0 \end{gathered}$$
但自动驾驶和机器人领域所用的MEMS多数达不到这种精度，因为角速度随机游走和加速度的随机游走交到，因此误差方程常写为
$$\begin{gathered} \delta {\dot{b}}_{a_t}=n_{b_a} \\ \delta {\dot{b}}_{{\omega }_t}=n_{b\omega } \end{gathered}$$
其中$n_{b_a}$和$n_{b_{\omega }}$分别为加速度计和陀螺仪的随机游走对应的白噪声。

• 惯性导航误差分析总结

$$\begin{gathered} \delta \dot{p}=\delta v \\ \delta \dot{v}=-R_t[{\hat{a}}_t-b_{a_t}{]}_{\times }\delta \theta -R_t(n_a+\delta b_{a_t}) \\ \delta \dot{\theta }=-[{\hat{\omega }}_t-b_{{\omega }_t}{]}_{\times }\delta \theta -n_{\omega }-\delta b_{\omega t} \\ \delta {\dot{b}}_{a_t}=n_{b_a} \\ \delta {\dot{b}}_{{\omega }_t}=n_{b\omega } \\ {\left[\begin{array}{c}\delta {\dot{\alpha }}_t^{b_k}\\ \delta {\dot{\beta }}_t^{b_k}\\ \delta {\dot{\theta }}_t^{b_k}\\ \delta {\dot{b}}_{a_t}\\ \delta {\dot{b}}_{w_t}\end{array}\right]}_{15\times 1}={\left[\begin{array}{ccccc}0& I& 0& 0& 0\\ 0& 0& -R_t^{b_k}[{\hat{a}}_t-b_{a_t}{]}_{\times }& -R_t^{b_k}& 0\\ 0& 0& -[{\hat{\omega }}_t-b_{w_t}{]}_{\times }& 0& -I\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]}_{15\times 15}\left[\begin{array}{c}\delta {\alpha }_t^{b_k}\\ \delta {\beta }_t^{b_k}\\ \delta {\theta }_t^{b_k}\\ \delta b_{a_t}\\ \delta b_{w_t}\end{array}\right]+{\left[\begin{array}{cccc}0& 0& 0& 0\\ -R_t^{b_k}& 0& 0& 0\\ 0& -I& 0& 0\\ 0& 0& I& 0\\ 0& 0& 0& I\end{array}\right]}_{15\times 12}{\left[\begin{array}{c}{n}_a\\ n_w\\ n_{b_a}\\ n_{b_w}\end{array}\right]}_{12\times 1} \\ =F_t\delta z_t^{b_k}+G_t{n}_t \end{gathered}$$

基于中值积分的离散时间预积分误差协方差递推式

• 从$\delta x_k$到$\delta x_{k+1}$ ：
  $$\begin{gathered} \delta x_{k+1}=\delta x_k+\delta {\dot{x}}_k\Delta t=\delta x_k+(F_t\delta x_k+G_t{n}_t)\Delta t \\ =(I+F_t\Delta t)\delta x_k+G_t\Delta t\cdot n_t=F\delta x_k+G{n}_k \end{gathered}$$
  下面将推导离散时间预积分误差传递方程
  $$\begin{gathered} \left[\begin{array}{c}\delta {\alpha }_{k+1}\\ \delta {\theta }_{k+1}\\ \delta {\beta }_{k+1}\\ \delta b_{a_{k+1}}\\ \delta b_{w_{k+1}}\end{array}\right]= \\ \left[\begin{array}{ccccc}I& f_{01}& \Delta t& f_{03}& f_{04}\\ 0& f_{11}& 0& 0& -\Delta t\\ 0& f_{21}& I& f_{23}& f_{24}\\ 0& 0& 0& I& 0\\ 0& 0& 0& 0& I\end{array}\right]\left[\begin{array}{c}\delta {\alpha }_k\\ \delta {\theta }_k\\ \delta {\beta }_k\\ \delta b_{a_k}\\ \delta b_{w_k}\end{array}\right]+\left[\begin{array}{cccccc}{v}_{00}& v_{01}& v_{02}& v_{03}& 0& 0\\ 0& \frac{-\Delta t}{2}& 0& \frac{-\Delta t}{2}& 0& 0\\ \frac{-R_k\Delta t}{2}& v_{21}& \frac{-R_{k+1}\Delta t}{2}& v_{23}& 0& 0\\ 0& 0& 0& 0& \Delta t& 0\\ 0& 0& 0& 0& 0& \Delta t\end{array}\right]\left[\begin{array}{c}{n}_{ak}\\ n_{wk}\\ n_{a_{k+1}}\\ n_{w_{k+1}}\\ n_{b_a}\\ n_{b_w}\end{array}\right] \end{gathered}$$
  其中，
  $$\begin{array}{rl}& f_{01}=\frac{\Delta t}{2}{f}_{21}=-\frac{1}{4}{R}_k({\hat{a}}_k-b_{ak}{)}^{\wedge }\Delta t^2-\frac{1}{4}{R}_{k+1}({\hat{a}}_{k+1}-b_{ak}{)}^{\wedge }[I-(\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{a}k{)}^{\wedge }\Delta t]\Delta t^2\\ & f_{03}=-\frac{1}{4}(R_k+R_{k+1})\Delta t^2\\ & f_{04}=\frac{\Delta t}{2}{f}_{24}=\frac{1}{4}{R}_{k+1}({\hat{a}}_{k+1}-b_{ak}{)}^{\wedge }\Delta t^3\\ & f_{11}=I-(\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{wk}{)}^{\wedge }\Delta t\\ & f_{21}=-\frac{1}{2}{R}_k({\hat{a}}_k-b_{ak}{)}^{\wedge }\Delta t-\frac{1}{2}{R}_{k+1}({\hat{a}}_{k+1}-b_{ak}{)}^{\wedge }[I-(\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{wk}{)}^{\wedge }\Delta t]\Delta t\\ & f_{23}=-\frac{1}{2}(R_k+R_{k+1})\Delta t\\ & f_{24}=\frac{1}{2}{R}_{k+1}({\hat{a}}_{k+1}-b_{ak}{)}^{\wedge }\Delta t^2\\ & v_{00}=-\frac{1}{4}{R}_k\Delta t^2\\ & v_{01}=v_{03}=\frac{\Delta t}{2}{v}_{21}=\frac{\Delta t}{2}\cdot \frac{1}{4}{R}_{k+1}({\hat{a}}_{k+1}-b_{ak}{)}^{\wedge }\Delta t^2\\ & v_{21}=v_{23}=\frac{1}{4}{R}_{k+1}({\hat{a}}_{k+1}-b_{ak}{)}^{\wedge }\Delta t^2\\ & v_{02}=-\frac{1}{4}{R}_{k+1}\Delta t^2\end{array}$$
  （1）$\delta {\theta }_{k+1}$的求解

  在连续时间下有
  $$\delta \dot{\theta }=-[{\hat{w}}_t-b_{wt}{]}_{\times }\delta \theta +n_w-\delta b_{wt}$$
  则离散时间下，利用中值积分有
  $$\delta {\dot{\theta }}_k=-[\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}{]}_{\times }\delta {\theta }_k+\frac{n_{w_k}+n_{w_{k+1}}}{2}-\delta b_{w_k}$$
  根据$\delta {\theta }_{k+1}=\delta {\theta }_k+\delta \dot{\theta }\Delta t$，有
  $$\delta {\theta }_{k+1}=[I-[\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}{]}_{\times }\Delta t]\delta {\theta }_k+\Delta t\frac{n_{w_k}+n_{w_{k+1}}}{2}-\Delta t\delta b_{w_k}$$

  （2）$\delta {\beta }_{k+1}$的求解

  在连续时间下有

  $$\delta \dot{\beta }=-R_t[{\hat{a}}_t-b_{a_t}{]}_{\times }\delta \theta +R_t(n_a-\delta b_{a_t})$$
  则在离散时间下，利用中值积分有
  $$\delta {\dot{\beta }}_k=-\frac{1}{2}{R}_k[a_k-b_{a_k}{]}_{\times }\delta {\theta }_k-\frac{1}{2}{R}_{k+1}[a_{k+1}-b_{a_k}{]}_{\times }\delta {\theta }_{k+1}+\frac{1}{2}{R}_k{n}_{a_k}+\frac{1}{2}{R}_{k+1}{n}_{a_{k+1}}-\frac{1}{2}(R_k+R_{k+1})\delta b_{a_k}$$
  将$\delta {\theta }_{k+1}$带入上式并合并可得到
  $$\begin{gathered} \delta {\dot{\beta }}_k=-\frac{1}{2}[R_k[a_k-b_{ak}{]}_{\times }+R_{k+1}[a_{k+1}-b_{ak}{]}_{\times }(I-[(\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{wk}{)}_{\times }]\Delta t)]\delta {\theta }_k-\frac{\Delta t}{4}{R}_{k+1}[a_{k+1}-b_{ak}{]}_{\times }{n}_{wk} \\ -\frac{\Delta t}{4}{R}_{k+1}[a_{k+1}-b_{ak}{]}_{\times }{n}_{wk+1}+\frac{\Delta t}{2}{R}_{k+1}[a_{k+1}-b_{ak}{]}_{\times }\delta b_{wk}+\frac{1}{2}{R}_k{n}_{ak}+\frac{1}{2}{R}_{k+1}{n}_{ak+1}-\frac{1}{2}(R_k+R_{k+1})\delta b_{ak} \end{gathered}$$
  根据 $\delta {\beta }_{k+1}={\beta }_k+\delta \dot{\beta }\Delta t$，有
  δ β k + 1 = δ β − { 1 2 R k ( a ^ k − b a k ) ∧ Δ t − 1 2 R k + 1 ( a ^ k + 1 − b a k ) ∧ [ I − ( w ^ k + w ^ k + 1 2 − b w k ) ∧ Δ t ] Δ t } δ θ k − 1 2 ( R k + R k + 1 ) Δ t δ b a k + 1 2 R k + 1 ( a ^ k + 1 − b a k ) ∧ Δ t 2 δ b w k − Δ t 2 4 R k + 1 [ a k + 1 − b a k ] × n w k − Δ t 2 4 R k + 1 [ a k + 1 − b a k ] × n w k + 1 + Δ t 2 R k n a k + Δ t 2 R k + 1 n a k + 1 \delta \beta_{k+1} = \delta \beta -\{\frac{1}{2}R_k(\hat a_k-b_{ak})^\wedge \Delta t - \frac{1}{2}R_{k+1}(\hat a_{k+1} - b_{ak})^\wedge [I - (\frac{\hat w_k + \hat w_{k+1}}{2} - b_{wk})^\wedge \Delta t]\Delta t\}\delta \theta_k \\ -\frac{1}{2} (R_k+R_{k+1})\Delta t \delta b_{ak} + \frac{1}{2}R_{k+1}(\hat a_{k+1} - b_{ak})^\wedge \Delta t^2 \delta b_{wk} - \frac{\Delta t^2}{4}R_{k+1}[a_{k+1}-b_{ak}]_\times n_{wk} \\ - \frac{\Delta t^2}{4}R_{k+1}[a_{k+1}-b_{ak}]_\times n_{wk+1} +\frac{\Delta t }{2}R_kn_{ak} +\frac{\Delta t}{2}R_{k+1}n_{ak+1} δβk+1​=δβ−{21​Rk​(a^k​−bak​)∧Δt−21​Rk+1​(a^k+1​−bak​)∧[I−(2w^k​+w^k+1​​−bwk​)∧Δt]Δt}δθk​−21​(Rk​+Rk+1​)Δtδbak​+21​Rk+1​(a^k+1​−bak​)∧Δt2δbwk​−4Δt2​Rk+1​[ak+1​−bak​]×​nwk​−4Δt2​Rk+1​[ak+1​−bak​]×​nwk+1​+2Δt​Rk​nak​+2Δt​Rk+1​nak+1​
  （3）$\delta {\alpha }_{k+1}$的求解

  由于连续时间下有 $\delta {\dot{\alpha }}_t=\delta {\beta }_t$，则在离散时间下有
  $$\delta {\dot{\alpha }}_k=\frac{1}{2}\delta {\beta }_k+\frac{1}{2}\delta {\beta }_{k+1}$$
  将上面推出的 $\delta {\beta }_{k+1}$ 带入，并根据$\delta {\alpha }_{k+1}=\delta {\alpha }_k+\delta {\dot{\alpha }}_k\Delta t$ 即可计算出$\delta {\alpha }_{k+1}$

• 预积分协方差矩阵的传递

  由上面可以得到
  $$\delta x_{k+1}=F_k\cdot \delta x_k+G_k\cdot n_k$$
  设$\delta x$服从高斯分布，$n_k\sim N(0,V_k)$，则$\delta x_{k+1}$的协方差为
  $$P_{k+1}=F_k{P}_k{F}_k^T+G_k{V}_k{G}_k^T$$