多传感器融合定位十三-基于图优化的建图方法其二

Reference:

1. 深蓝学院-多传感器融合
2. 多传感器融合定位理论基础

文章跳转：

1. 多传感器融合定位一-3D激光里程计其一：ICP
2. 多传感器融合定位二-3D激光里程计其二：NDT
3. 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
4. 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
5. 多传感器融合定位五-点云地图构建及定位
6. 多传感器融合定位六-惯性导航原理及误差分析
7. 多传感器融合定位七-惯性导航解算及误差分析其一
8. 多传感器融合定位八-惯性导航解算及误差分析其二
9. 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
10. 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
11. 多传感器融合定位十一-基于滤波的融合方法Ⅱ
12. 多传感器融合定位十二-基于图优化的建图方法其一
13. 多传感器融合定位十三-基于图优化的建图方法其二
14. 多传感器融合定位十四-基于图优化的定位方法
15. 多传感器融合定位十五-多传感器时空标定(综述)

3.4 预积分方差计算

3.4.1 核心思路

在融合时，需要给不同信息设置权重，而权重由方差得来，因此对于IMU积分，也要计算其方差。方差的计算形式与第四章相同，即：
$${\mathbf{P}}_{i,k+1}={\mathbf{F}}_k{\mathbf{P}}_{i,k}{\mathbf{F}}_k^{\top }+{\mathbf{B}}_k\mathbf{Q}{\mathbf{B}}_k^{\top }$$但需注意的是，此处 ${\mathbf{F}}_k$ 和 ${\mathbf{G}}_k$ 是离散时间下的状态传递方程中的矩阵，而我们一般是在连续时间下推导微分方程，再用它计算离散时间下的传递方程。

3.4.2 连续时间下的微分方程

连续时间下的微分方程一般写为如下形式：
$$\begin{array}{c}\dot{\mathbf{x}}={\mathbf{F}}_t\mathbf{x}+{\mathbf{B}}_t\mathbf{w}\\ \mathbf{x}=\left[\begin{array}{l}\delta {\mathbf{\alpha }}_t^{b_k}\\ \delta {\mathbf{\theta }}_t^{b_k}\\ \delta {\mathbf{\beta }}_t^{b_k}\\ \delta {\mathbf{b}}_{{\mathbf{a}}_t}\\ \delta {\mathbf{b}}_{w_t}\end{array}\right]\\ \mathbf{w}=\left[\begin{array}{l}{\mathbf{n}}_a\\ {\mathbf{n}}_w\\ {\mathbf{n}}_{b_a}\\ {\mathbf{n}}_{b_w}\end{array}\right]\end{array}$$以上变量排列顺序是为了与lio/vio等常见系统顺序保持一致。

推导方法已经在第6讲介绍，此处直接给出结果：
$\delta {\dot{\mathbf{\theta }}}_t^{b_k}$ 的微分方程：
$$\delta {\dot{\mathbf{\theta }}}_t^{b_k}=-{\left[{\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\right]}_{\times }\delta {\mathbf{\theta }}_t^{b_k}+{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{{\omega }_t}$$$\delta {\dot{\mathbf{\beta }}}_t^{b_k}$ 的微分方程：
$$\delta {\dot{\mathbf{\beta }}}_t^{b_k}=-{\mathbf{R}}_t{\left[{\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right]}_{\times }\delta {\mathbf{\theta }}_t^{b_k}+{\mathbf{R}}_t\left({\mathbf{n}}_a-\delta {\mathbf{b}}_{a_t}\right)$$$\delta {\dot{\mathbf{\alpha }}}_t^{b_k}$ 的微分方程：
$$\delta {\dot{\mathbf{\alpha }}}_t^{b_k}=\delta {\mathbf{\beta }}_t^{b_k}$$

3.4.3 离散时间下的传递方程

得到连续时间微分方程以后，就可以计算离散时间的递推方程了，表示为：
$${\mathbf{x}}_{k+1}={\mathbf{F}}_k{\mathbf{x}}_k+{\mathbf{B}}_k{\mathbf{w}}_k$$其中：
$${\mathbf{x}}_{k+1}=\left[\begin{array}{l}\delta {\mathbf{\alpha }}_{k+1}\\ \delta {\mathbf{\theta }}_{k+1}\\ \delta {\mathbf{\beta }}_{k+1}\\ \delta {\mathbf{b}}_{a_{k+1}}\\ \delta {\mathbf{b}}_{{\omega }_{k+1}}\end{array}\right]{\mathbf{x}}_k=\left[\begin{array}{c}\delta {\mathbf{\alpha }}_k\\ \delta {\mathbf{\theta }}_k\\ \delta {\mathbf{\beta }}_k\\ \delta {\mathbf{b}}_{a_k}\\ \delta {\mathbf{b}}_{{\omega }_k}\end{array}\right]{\mathbf{w}}_k=\left[\begin{array}{c}{\mathbf{n}}_{a_k}\\ {\mathbf{n}}_{w_k}\\ {\mathbf{n}}_{a_{k+1}}\\ {\mathbf{n}}_{w_{k+1}}\\ {\mathbf{n}}_{b_a}\\ {\mathbf{n}}_{b_w}\end{array}\right]$$

1. $\delta {\mathbf{\theta }}_{k+1}$ 的求解
   由于连续时间下有
   $$\delta \dot{\mathbf{\theta }}=-{\left[{\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{{\omega }_t}$$则离散时间下应该有
   因此有
   $$\begin{gathered} \delta {\dot{\mathbf{\theta }}}_k=-{\left[\frac{{\mathbf{\omega }}_k+{\mathbf{\omega }}_{k+1}}{2}-{\mathbf{b}}_{{\omega }_t}\right]}_{\times }\delta {\mathbf{\theta }}_k+\frac{{\mathbf{n}}_{{\omega }_k}+{\mathbf{n}}_{{\omega }_{k+1}}}{2}-\delta {\mathbf{b}}_{{\omega }_k} \\ \delta {\mathbf{\theta }}_{k+1}=\left[\mathbf{I}-{\left[\frac{{\mathbf{\omega }}_k+{\mathbf{\omega }}_{k+1}}{2}-{\mathbf{b}}_{{\omega }_k}\right]}_{\times }\delta t\right]\delta {\mathbf{\theta }}_k+\delta t\frac{{\mathbf{n}}_{{\omega }_k}+{\mathbf{n}}_{{\omega }_{k+1}}}{2}-\delta t\delta {\mathbf{b}}_{{\omega }_k} \end{gathered}$$令 $\overline{\mathbf{\omega }}=\frac{{\omega }_k+{\omega }_{k+1}}{2}-{\mathbf{b}}_{{\omega }_k}$ ，则上式可以重新写为
   $$\delta {\mathbf{\theta }}_{k+1}=\left[\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right]\delta {\mathbf{\theta }}_k+\frac{\delta t}{2}{\mathbf{n}}_{{\omega }_k}+\frac{\delta t}{2}{\mathbf{n}}_{{\omega }_{k+1}}-\delta t\delta {\mathbf{b}}_{{\omega }_k}$$

2. $\delta {\mathbf{\beta }}_{k+1}$ 的求解
   由于连续时间下有
   $$\delta \dot{\mathbf{\beta }}=-{\mathbf{R}}_t{\left[{\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{R}}_t\left({\mathbf{n}}_a-\delta {\mathbf{b}}_{a_t}\right)$$
   则离散时间下应该有
   $$\begin{array}{rl}\delta {\dot{\mathbf{\beta }}}_k=& -\frac{1}{2}{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{\theta }}_k\\ & -\frac{1}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{\theta }}_{k+1}\\ & +\frac{1}{2}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{1}{2}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{1}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$把前面求得的 $\delta {\mathbf{\theta }}_{k+1}$ 的表达式代入上式，可得
   $$\begin{array}{rl}\delta {\dot{\mathbf{\beta }}}_k=& -\frac{1}{2}{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{\theta }}_k\\ & -\frac{1}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left\{\left[\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right]\delta {\mathbf{\theta }}_k+\frac{\delta t}{2}{\mathbf{n}}_{{\omega }_k}+\frac{\delta t}{2}{\mathbf{n}}_{{\omega }_{k+1}}-\delta t\delta {\mathbf{b}}_{{\omega }_k}\right\}\\ & +\frac{1}{2}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{1}{2}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{1}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$经过一系列合并同类项以后，最终的合并结果为
   $$\begin{array}{rl}\delta {\dot{\mathbf{\beta }}}_k=& -\frac{1}{2}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\delta {\mathbf{\theta }}_k\\ & -\frac{\delta t}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_k}\\ & -\frac{\delta t}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_{k+1}}\\ & +\frac{\delta t}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{b}}_{{\omega }_k}\\ & +\frac{1}{2}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{1}{2}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{1}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$由导数形式可以得到递推形式如下
   $$\begin{array}{rl}\delta {\mathbf{\beta }}_{k+1}=& \delta {\mathbf{\beta }}_k\\ & -\frac{\delta t}{2}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\delta {\mathbf{\theta }}_k\\ & -\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_k}\\ & -\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_{k+1}}\\ & +\frac{\delta t^2}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{b}}_{{\omega }_k}\\ & +\frac{\delta t}{2}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{\delta t}{2}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{\delta t}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$

3. $\delta {\mathbf{\alpha }}_{k+1}$ 的求解
   由于连续时间下有 $\delta {\dot{\mathbf{\alpha }}}_t=\delta {\mathbf{\beta }}_t$ ，则离散时间下应该有
   $$\begin{array}{rl}\delta {\dot{\mathbf{\alpha }}}_k=& \frac{1}{2}\delta {\mathbf{\beta }}_k+\frac{1}{2}\delta {\mathbf{\beta }}_{k+1}\\ =& \delta {\mathbf{\beta }}_k\\ & -\frac{\delta t}{4}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\delta {\mathbf{\theta }}_k\\ & -\frac{\delta t^2}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{{\mathbf{a}}_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_k}\\ & -\frac{\delta t^2}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{{\mathbf{a}}_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_{k+1}}\\ & +\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{{\mathbf{a}}_k}\right]\times \delta {\mathbf{b}}_{{\omega }_k}\\ & +\frac{\delta t}{4}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{\delta t}{4}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{\delta t}{4}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$由导数形式可以写出递推形式
   $$\begin{array}{rl}\delta {\mathbf{\alpha }}_{k+1}=& \delta {\mathbf{\alpha }}_k\\ & +\delta t\delta {\mathbf{\beta }}_k\\ & -\frac{\delta t^2}{4}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\bar{\omega }{]}_{\times }\delta t\right)\right]\delta {\mathbf{\theta }}_k\\ & -\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_k}\\ & -\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }{\mathbf{n}}_{{\omega }_{k+1}}\\ & +\frac{\delta t^3}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{b}}_{{\omega }_k}\\ & +\frac{\delta t^2}{4}{\mathbf{R}}_k{\mathbf{n}}_{a_k}\\ & +\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\mathbf{n}}_{a_{k+1}}\\ & -\frac{\delta t^2}{4}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta {\mathbf{b}}_{a_k}\end{array}$$由以上的推导结果，便可以写出 ${\mathbf{x}}_{k+1}={\mathbf{F}}_k{\mathbf{x}}_k+{\mathbf{B}}_k{\mathbf{w}}_k$ 中的矩阵
   $$\begin{array}{rl}& {\mathbf{F}}_k=\left[\begin{array}{ccccc}\mathbf{I}& {\mathbf{f}}_{12}& \mathbf{I}\delta t& -\frac{1}{4}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta t^2& {\mathbf{f}}_{15}\\ 0& \mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t& 0& 0& -\mathbf{I}\delta t\\ 0& {\mathbf{f}}_{32}& \mathbf{I}& -\frac{1}{2}\left({\mathbf{R}}_k+{\mathbf{R}}_{k+1}\right)\delta t& {\mathbf{f}}_{35}\\ 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& \mathbf{I}\end{array}\right]\\ & {\mathbf{B}}_k=\left[\begin{array}{cccccc}\frac{1}{4}{\mathbf{R}}_k\delta t^2& {\mathbf{g}}_{12}& \frac{1}{4}{\mathbf{R}}_{k+1}\delta t^2& {\mathbf{g}}_{14}& 0& 0\\ 0& \frac{1}{2}\mathbf{I}\delta t& 0& \frac{1}{2}\mathbf{I}\delta t& 0& 0\\ \frac{1}{2}{\mathbf{R}}_k\delta t& {\mathbf{g}}_{32}& \frac{1}{2}{\mathbf{R}}_{k+1}\delta t& {\mathbf{g}}_{34}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}\delta t& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\delta t\end{array}\right]\end{array}$$上面的矩阵中，有
   $$\begin{array}{rl}& {\mathbf{f}}_{12}=-\frac{\delta t^2}{4}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\\ & {\mathbf{f}}_{15}=\frac{\delta t^3}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\delta {\mathbf{b}}_{{\omega }_k}\\ & {\mathbf{f}}_{32}=-\frac{\delta t}{2}\left[{\mathbf{R}}_k{\left[{\mathbf{a}}_k-{\mathbf{b}}_{a_k}\right]}_{\times }+{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\left(\mathbf{I}-[\overline{\mathbf{\omega }}{]}_{\times }\delta t\right)\right]\\ & {\mathbf{f}}_{35}=\frac{\delta t^2}{2}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\\ & {\mathbf{g}}_{12}=-\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\\ & {\mathbf{g}}_{14}=-\frac{\delta t^3}{8}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\\ & {\mathbf{g}}_{32}=-\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\\ & {\mathbf{g}}_{34}=-\frac{\delta t^2}{4}{\mathbf{R}}_{k+1}{\left[{\mathbf{a}}_{k+1}-{\mathbf{b}}_{a_k}\right]}_{\times }\end{array}$$

3.5 预积分更新

回到bias变化时，预积分结果怎样重新计算的 问题，再次给出它的泰勒展开形式：
$$\begin{array}{c}{\mathbf{\alpha }}_{b_i{b}_j}={\overline{\mathbf{\alpha }}}_{b_i{b}_j}+{\mathbf{J}}_{b_i^a}^{\alpha }\delta {\mathbf{b}}_i^a+{\mathbf{J}}_{b_i^g}^{\alpha }\delta {\mathbf{b}}_i^g\\ {\mathbf{\beta }}_{b_i{b}_j}={\overline{\mathbf{\beta }}}_{b_i{b}_j}+{\mathbf{J}}_{b_i^a}^{\beta }\delta {\mathbf{b}}_i^a+{\mathbf{J}}_{b_i^g}^{\beta }\delta {\mathbf{b}}_i^g\\ {\mathbf{q}}_{b_i{b}_j}={\overline{\mathbf{q}}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i^q}^q\delta {\mathbf{b}}_i^g\end{array}\right]\end{array}$$这里，雅可比没有明确的闭式解，但是在推导 方差的更新时，我们得到了
$${\mathbf{x}}_{k+1}={\mathbf{F}}_k{\mathbf{x}}_k+{\mathbf{B}}_k{\mathbf{w}}_k$$通过该递推形式，可以知道
$${\mathbf{J}}_{k+1}={\mathbf{F}}_k{\mathbf{J}}_k$$即预积分结果的雅可比，可以通过这种迭代方式计算。
${\mathbf{J}}_j$ 中关于 bias 的项，就是预积分泰勒展开时，各 bias 对应的雅可比。

4. 典型方案介绍

4.1 LIO-SAM介绍

论文名称：LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
代码地址：https://github.com/TixiaoShan/LIO-SAM

最大特点：分两步完成，先通过点云特征计算出相对位姿，再利用相对位姿、IMU预积分和GPS做融合。

相比于直接一步做紧耦合，大大提高了效率，而且实测性能也很优异。

5. 融合编码器的优化方案

5.1 整体思路介绍

理论上，只要是在载体系下测量，且频率比关键帧的提取频率高，就都可以做预积分。
编码器与IMU基于预积分的融合有多种方法，此处选相对简单的一种：
把它们当做一个整体的传感器来用，IMU提供角速度，编码器提供位移增量，且不考虑编码器的误差。
此时对预积分模型，输入为
角速度： ${\omega }_k=\left[\begin{array}{l}{\omega }_{xk}\\ {\omega }_{yk}\\ {\omega }_{zk}\end{array}\right]$ 位移增量: ${\varphi }_k=\left[\begin{array}{c}{\varphi }_{xk}\\ 0\\ 0\end{array}\right]$
输出中包含位置、姿态，但不包含速度。

5.2 预积分模型设计

连续时间下，从 $i$ 时刻到 $j$ 时刻 IMU的积分结果为
$$\begin{array}{rl}{\mathbf{p}}_{w{b}_j}& ={\mathbf{p}}_{w{b}_i}+{\int }_{t\in [i,j]}{\mathbf{q}}_{w{b}_t}{\mathbf{\varphi }}^{b_t}\delta t\\ {\mathbf{q}}_{w{b}_j}& ={\int }_{t\in [i,j]}{\mathbf{q}}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$把 ${\mathbf{q}}_{w{b}_t}={\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_t}$ 代入上式可得
$$\begin{gathered} {\mathbf{p}}_{w{b}_j}={\mathbf{p}}_{w{b}_i}+{\mathbf{q}}_{w{b}_i}{\int }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{\varphi }}^{b_t}\right)\delta t \\ {\mathbf{q}}_{w{b}_j}={\mathbf{q}}_{w{b}_i}[{\int }_{t\in [i,j]}{\mathbf{q}}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t \end{gathered}$$为了整理公式，把积分相关的项用下面的式子代替
$$\begin{array}{rl}& {\mathbf{\alpha }}_{b_i{b}_j}={\int }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{\varphi }}^{b_t}\right)\delta t\\ & {\mathbf{q}}_{b_i{b}_j}={\int }_{t\in [i,j]}{\mathbf{q}}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$使用中值积分方法，把连续方法改成离散形式如下
$$\begin{array}{rl}{\mathbf{\omega }}^b& =\frac{1}{2}\left[\left({\mathbf{\omega }}^{b_k}-{\mathbf{b}}_k^g\right)+\left({\mathbf{\omega }}^{b_{k+1}}-{\mathbf{b}}_k^g\right)\right]\\ {\mathbf{\varphi }}^w& =\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}{\mathbf{\varphi }}^{b_k}+{\mathbf{q}}_{b_i{b}_{k+1}}{\mathbf{\varphi }}^{b_{k+1}}\right)\end{array}$$那么预积分的离散形式可以表示为
$$\begin{array}{rl}& {\mathbf{\alpha }}_{b_i{b}_{k+1}}={\mathbf{\alpha }}_{b_i{b}_k}+{\mathbf{\varphi }}^w\delta t\\ & {\mathbf{q}}_{b_i{b}_{k+1}}={\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{\omega }}^b\delta t\end{array}\right]\end{array}$$此时状态更新的公式可以整理为
$$\left[\begin{array}{c}{\mathbf{p}}_{w{b}_j}\\ {\mathbf{q}}_{w{b}_j}\\ {\mathbf{b}}_j^{g_j}\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_{w{b}_i}+{\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}\\ {\mathbf{q}}_{w{b}_i}{\mathbf{q}}_{b_i{b}_j}\\ {\mathbf{b}}_i^g\end{array}\right]$$残差形式可以写为
$$\left[\begin{array}{c}{\mathbf{r}}_p\\ {\mathbf{r}}_q\\ {\mathbf{r}}_{bg}\end{array}\right]=\left[\begin{array}{c}{\mathbf{q}}_{w{b}_i}^{\ast }\left({\mathbf{p}}_{w{b}_j}-{\mathbf{p}}_{w{b}_i}\right)-{\mathbf{\alpha }}_{b_{b^b}{b}_j}\\ 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}\\ {\mathbf{b}}_j^g-{\mathbf{b}}_i^g\end{array}\right]$$其余部分(方差递推、残差对状态量雅可比、bias更新等)的推导，留作作业(仅优秀标准需要做)。