当前位置：首页 > article >正文

《自动驾驶与机器人中的SLAM技术》ch4：预积分学

article 2025/1/17 23:58:33

1 预积分的定义

2 预积分的测量模型 ( 预积分的测量值可由 IMU 的测量值积分得到 )

2.1 旋转部分

2.2 速度部分

2.3 平移部分

2.4 将预积分测量和误差式代回最初的定义式

3 预积分的噪声模型和协方差矩阵

3.1 旋转部分

3.2 速度部分

3.3 平移部分

3.4 噪声项合并

4 零偏的更新

5 图优化中的预积分边

5.1 预积分边的残差

5.2 预积分残差相对于状态变量的雅可比矩阵

5.2.1 旋转部分

5.2.2 速度部分

5.2.3 平移部分

6 预积分流程

7 预积分的程序实现

7.1 程序实现

7.2 测试

1 测试在恒定角速度运转下的预积分情况

2. 测试在恒定加速度运行下的预积分情况

在基于 ESKF 的 GINS 中，我们将两个 GNSS 观测之间的 IMU 数据进行积分，作为 ESKF 的预测过程。这种做法把 IMU 数据看成某种一次性的使用方式：将它们积分到当前状态估计值上，然后用观测数据更新当时的状态估计值。这种做法和此时的状态估计值有关。但是，如果状态量发生了改变，能否重复利用这些 IMU 数据呢？从物理意义上看，IMU 反映的是两个时刻间车辆的角度变化量和速度变化量。如果我们希望 IMU 的计算与当时的状态估计值无关，在算法上应该如何处理呢？这就是本章要讨论的内容。

本章介绍一种十分常见的 IMU 数据处理方法：预积分（Pre-integraion）。与传统 IMU 的运动学积分不同，预积分可以将一段时间内的 IMU 测量数据累计起来，建立预积分测量，同时还能保证测量值与状态变量无关。如果以吃饭来比喻的话，ESKF 像是一口口地吃菜，而预积分则是从锅里先把菜一块块夹到碗里，然后再把碗里的菜一口气吃掉。至于用多大的碗，每次夹多少次菜再一起吃，形式上就比较自由了。无论是 LIO 系统还是 VIO 系统，预积分已经成为诸多与 IMU 紧耦合的标准方法，但是原理相对传统 ESKF 的预测过程会更加复杂一些。下面我们来推导其基本原理，然后实现一个预积分系统。

1 预积分的定义

在一个 IMU 系统里，我们考虑它的五个变量：旋转 R、平移 p、角速度 ω、线速度 v 与加速度 a。根据第 2 章介绍的运动学，这些变量的运动学关系可以写成如下运动学方程：

$\begin{aligned} & \dot{R}=R\omega^{\wedge}, \\ & \dot{p}=v, \\ & \dot{v}=a. \end{aligned}$

在 $t$ 到 $t+\Delta t$ 时间内对运动学方程进行欧拉积分得：

$\begin{aligned} & {R}(t+\Delta t)=R(t)\mathrm{Exp}(\omega(t)\Delta t), \\ & {v}(t+\Delta t)=v(t)+{a}(t)\Delta t, \\ & {p}(t+\Delta t)=p(t)+v(t)\Delta t+\frac{1}{2}\boldsymbol{a}(t)\Delta t^{2}. \end{aligned}$

IMU 测量方程（其中 ${\eta}_{g}$ ， ${\eta}_{a}$ 为 IMU 测量的高斯噪声）如下：

$\begin{aligned} & \tilde{\boldsymbol{\omega}}(t)=\boldsymbol{\omega}(t)+\boldsymbol{b}_{g}(t)+\boldsymbol{\eta}_{g}(t), \\ & \tilde{\boldsymbol{a}}(t)=\boldsymbol{R}^\top(\boldsymbol{a}(t)-\boldsymbol{g})+\boldsymbol{b}_a(t)+\boldsymbol{\eta}_a(t), \end{aligned}$

IMU 测量方程带入积分后的运动学方程（其中 ${\eta}_{gd}$ ， ${\eta}_{ad}$ 为离散化后的 IMU 测量噪声）如下：

$\begin{aligned} & \boldsymbol{R}(t+\Delta t)=R(t)\mathrm{Exp}\left((\tilde{\omega}-b_{g}(t)-\eta_{gd}(t))\Delta t\right), \\ & \boldsymbol{v}(t+\Delta t)=\boldsymbol{v}(t)+\boldsymbol{g}\Delta t+\boldsymbol{R}(t)(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a}(t)-\boldsymbol{\eta}_{ad}(t))\Delta t, \\ & \boldsymbol{p}(t+\Delta t)=\boldsymbol{p}(t)+\boldsymbol{v}(t)\Delta t+\frac{1}{2}\boldsymbol{g}\Delta t^2+\frac{1}{2}\boldsymbol{R}(t)(\tilde{\boldsymbol{a}}-\boldsymbol{b}_a(t)-\boldsymbol{\eta}_{ad}(t))\Delta t^2, \end{aligned}$

其中噪声项满足：

$\begin{aligned} &\mathrm{Cov}(\boldsymbol{\eta}_{gd}(t))=\frac{1}{\Delta t}\mathrm{Cov}(\boldsymbol{\eta}_{g}(t)) \\& \mathrm{Cov}(\boldsymbol{\eta}_{ad}(t))=\frac{1}{\Delta t}\mathrm{Cov}(\boldsymbol{\eta}_{a}(t))\end{aligned}$

以上过程与我们在 IMU 测量方程和噪声方程（第 3 章）中已有描述。当然，我们完全可以用这种约束来构建图优化，对 IMU 相关的问题进行求解。但是这组方程刻画的时间太短，仅包含单个 IMU 数据。或者说，IMU 的测量频率太高。我们并不希望优化过程随着 IMU 数据进行调用，那样太浪费计算资源。我们更希望将这些 IMU 测量值组合在一起处理，即在关键帧之间对 IMU 数据进行预积分，然后按照关键帧的频率调用优化过程。

假设从离散时间 $i$ 和 $j$ 之间的 IMU 数据被累计起来，这个过程可以持续若干秒钟。这种被累计起来的观测被称为预积分。如果我们使用不同形式的运动学（第 2 章中介绍的运动学），得到的预积分形式也是不同的。本书主要使用 SO(3) + t 的方式来推导预积分。那么，累积离散时间 $i$ 和 $j$ 之间的 IMU 数据，令累积的时间为 $\Delta t_{ij}=\sum_{k=i}^{j-1}\Delta t$ ，得到：

$\begin{aligned} & {R}_j=R_{i}\prod_{k=i}^{j-1}(\mathrm{Exp}\left((\tilde{\omega}_{k}-b_{g,k}-\eta_{gd,k})\Delta t\right), \\ & \boldsymbol{v}_{j}=v_{i}+g\Delta t_{ij}+\sum_{k=i}^{j-1}R_{k}(\tilde{a}_{k}-b_{a,k}-\eta_{ad,k})\Delta t, \\ & p_{j}=p_{i}+\sum_{k=i}^{j-1}v_{k}\Delta t+\frac{1}{2}\sum_{k=i}^{j-1}g\Delta t^{2}+\frac{1}{2}\sum_{k=i}^{j-1}R_{k}(\tilde{a}_{k}-b_{a,k}-\eta_{ad,k})\Delta t^{2}, \end{aligned}$

上述式子只是累积形式，是传统意义上的直接积分。直接积分的缺点是其描述的过程和状态量有关。如果我们对 i 时刻的状态进行优化，那么 $i+1$ , $i+2$ , . . . , $j-1$ 时刻的状态也会跟着发生改变，这个积分就必须重新计算，这是非常不便的。为此，定义状态变化量 $\Delta R_{ij},\Delta v_{ij},\Delta p_{ij}$ 如下：

$\begin{aligned} & \Delta R_{ij}\doteq R_{i}^{\top}R_{j} \\ & \Delta v_{ij}\overset{.}{\operatorname*{=}}R_{i}^{\top}(v_{j}-v_{i}-g\Delta t_{ij}) \\ & \Delta p_{ij}\overset{.}{\operatorname*{=}}R_{i}^{\top}\left(p_{j}-p_{i}-v_{i}\Delta t_{ij}-\frac{1}{2}\sum_{k=i}^{j-1}g\Delta t^{2}\right) \end{aligned}$

对上述累积式稍加改变，尽量将 IMU 读数放在一侧，状态量放到另一侧，可得如下式子，该式称为预积分的定义式：

$\begin{aligned} & \Delta R_{ij}\doteq R_{i}^{\top}R_{j}=\prod_{k=i}^{j-1}\mathrm{Exp}\left(\left(\tilde{\omega}_{k}-b_{g,k}-\eta_{gd,k}\right)\Delta t\right), \\ & \Delta v_{ij}\overset{.}{\operatorname*{=}}R_{i}^{\top}(v_{j}-v_{i}-g\Delta t_{ij})=\sum_{k=i}^{j-1}\Delta R_{ik}(\tilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{a,k}-\boldsymbol{\eta}_{ad,k})\Delta t, \\ & \Delta p_{ij}\overset{.}{\operatorname*{=}}R_{i}^{\top}\left(p_{j}-p_{i}-v_{i}\Delta t_{ij}-\frac{1}{2}\sum_{k=i}^{j-1}g\Delta t^{2}\right) \\ & =\sum_{k=i}^{j-1}\left[\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}\left(\tilde{a}_{k}-b_{a,k}-\eta_{ad,k}\right)\Delta t^{2}\right]. \end{aligned}$

1. 我们不妨考虑从 $i$ 时刻出发，此时这三个量都为零。在 $i+1$ 时刻，我们计算出 $\Delta R_{i,i+1}$ , $\Delta v_{i,i+1}$ 和 $\Delta p_{i,i+1}$ 。而在 $i+2$ 时刻时，由于这三个式子都是累乘或累加的形式，只需在 $i$ , $i+1$ 时刻的结果之上，加上第 $i+2$ 时刻的测量值即可。这在计算层面带来了很大的便利。进一步，我们还会发现这种性质对后续计算各种雅可比矩阵都非常方便。
2. 从等号最右侧来看，上述所有计算都和 $R,v,p$ 的取值无关。即使它们的状态估计值发生改变，IMU 积分量也无需重新计算。
3. 不过，如果零偏 $b_{a,k}$ 或 $b_{g,k}$ 发生变化，那么上述式子理论上还是需要重新计算。然而，我们也可以通过“修正”而非“重新计算”的思路，来调整我们的预积分量。
4. 请注意，预积分量并没有直接的物理含义。尽管符号上用了 $\Delta v$ , $\Delta p$ 之类的样子，但它并不表示某两个速度或位置上的偏差。它只是如此定义而已。当然，从量纲上来说，应该与角度、速度、位移对应。
5. 同样地，由于预积分量不是直接的物理量，这种“预积分测量模型”的噪声也必须从原始的 IMU 噪声推导而来。

2 预积分的测量模型 ( 预积分的测量值可由 IMU 的测量值积分得到 )

由前面的讨论可见，预积分内部带有 IMU 的零偏量，因此不可避免地会依赖此时的零偏量估计。为了处理这种依赖，我们对预积分定义作一些工程上的调整：

1. 我们首先认为 $\boldsymbol {i}$ 时刻的零偏是固定的，并且在整个预积分计算过程中也都是固定的。
2. 假设预积分的测量值是随零偏线性变化的（一阶线性化模型），舍弃对零偏量的高阶项。
3. 当零偏估计发生改变时，在原先预积分测量值的基础上使用线性模型进行修正得到新的预积分测量值。

首先，我们固定 $i$ 时刻的零偏估计，来分析预积分的噪声。无论是图优化还是滤波器技术，都需要知道某个测量量究竟含有多大的噪声。

2.1 旋转部分

定义预积分旋转测量值 $\Delta\tilde{\boldsymbol{R}}_{ij}$ 如下：

$\begin{aligned} & \Delta\tilde{\boldsymbol{R}}_{ij}=\prod_{k=i}^{j-1}\operatorname{Exp}\left((\tilde{\boldsymbol{\omega}}_{k}-\boldsymbol{b}_{g,i})\Delta t\right) \end{aligned}$

预积分定义式中的旋转部分经过变换可得如下式，其中 $\delta\boldsymbol\phi_{ij}$ 为预积分旋转的测量噪声：

$\begin{aligned} & \Delta\boldsymbol{R}_{ij}=\prod_{k=i}^{j-1}\mathrm{Exp}\left(\left(\tilde{\omega}_{k}-b_{g,k}-\eta_{gd,k}\right)\Delta t\right), \\ &=\Delta\tilde{\boldsymbol{R}}_{ij}\prod_{k=i}^{j-1}\operatorname{Exp}\left(-\Delta\tilde{\boldsymbol{R}}_{k+1,j}^{\top}\boldsymbol{J}_{r,k}\boldsymbol{\eta}_{gd,k}\Delta t\right), \\ & \doteq\Delta\tilde{\boldsymbol{R}}_{ij}\mathrm{Exp}(-\delta\phi_{ij}). \end{aligned}$

上式中的 $\boldsymbol{J}_{r,k}=\boldsymbol{J}_{r}[(\tilde{\boldsymbol{\omega}}_{k}-\boldsymbol{b}_{g,i})\Delta t]$ ，可根据《自动驾驶与机器人中的SLAM技术》ch2：基础数学知识中公式计算得到

2.2 速度部分

定义预积分速度测量值 $\Delta\tilde{\boldsymbol{v}}_{ij}$ 如下：

$\begin{aligned} & \Delta\tilde{\boldsymbol{v}}_{ij}=\sum_{k=i}^{j-1}\Delta\tilde{\boldsymbol{R}}_{ik}(\tilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{a,i})\Delta t \end{aligned}$

预积分定义式中的速度部分经过变换可得如下式，其中 $\delta\boldsymbol v_{ij}$ 为预积分速度的测量噪声：

$\begin{aligned} & \mathrm\Delta\boldsymbol{v}_{ij} =\sum_{k=i}^{j-1}\Delta R_{ik}(\tilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{a,k}-\boldsymbol{\eta}_{ad,k})\Delta t, \\ &=\Delta\tilde{v}_{ij}+\sum_{k=i}^{j-1}\left[\Delta\tilde{\boldsymbol{R}}_{ik}(\tilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{a,i})^{\wedge}\delta\boldsymbol{\phi}_{ik}\Delta t-\Delta\tilde{\boldsymbol{R}}_{ik}\boldsymbol{\eta}_{ad,k}\Delta t\right] ,\\ & \doteq \Delta\tilde{\boldsymbol{v}}_{ij}-\delta\boldsymbol{v}_{ij} .\end{aligned}$

2.3 平移部分

定义预积分平移测量值 $\Delta\tilde{\boldsymbol{p}}_{ij}$ 如下：

$\begin{aligned} & \Delta\tilde{p}_{ij}=\sum_{k=i}^{j-1}\left[(\Delta\tilde{\boldsymbol{v}}_{ik}\Delta t)+\frac{1}{2}\Delta\tilde{\boldsymbol{R}}_{ik}(\tilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{a,i})\Delta t^{2}\right] \end{aligned}$

预积分定义式中的平移部分经过变换可得如下式，其中 $\delta\boldsymbol p_{ij}$ 为预积分平移的测量噪声：

$\begin{aligned} & \Delta p_{ij}=\sum_{k=i}^{j-1}\left[\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}\left(\tilde{a}_{k}-b_{a,k}-\eta_{ad,k}\right)\Delta t^{2}\right], \\&=\Delta\tilde{p}_{ij}+\sum_{k=i}^{j-1}\left[-\delta\boldsymbol{v}_{ik}\Delta t+\frac{1}{2}\Delta\tilde{\boldsymbol{R}}_{ik}(\tilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{a,i})^{\wedge}\delta\boldsymbol{\phi}_{ik}\Delta t^{2}-\frac{1}{2}\Delta\tilde{\boldsymbol{R}}_{ik}\boldsymbol{\eta}_{ad,k}\Delta t^{2}\right], \\ & \doteq\Delta\tilde{p}_{ij}-\delta\boldsymbol{p}_{ij}. \end{aligned}$

2.4 将预积分测量和误差式代回最初的定义式

将上面的预积分测量及误差式：

$\begin{aligned} & \Delta\boldsymbol{R}_{ij}=\Delta\tilde{\boldsymbol{R}}_{ij}\mathrm{Exp}(-\delta\phi_{ij}) \\ & \Delta\boldsymbol{v}_{ij}=\Delta\tilde{\boldsymbol{v}}_{ij}-\delta\boldsymbol{v}_{ij} \\& \Delta\boldsymbol{p}_{ij}=\Delta\tilde{\boldsymbol{p}}_{ij}-\delta\boldsymbol{p}_{ij} \end{aligned}$

重新代入最初的预积分定义式中，得：

$\begin{aligned} & \Delta\tilde{R}_{ij}=R_i^\top R_j\mathrm{Exp}(\delta\boldsymbol{\phi}_{ij}), \\ & \Delta\tilde{v}_{ij}=R_i^\top\left(v_j-v_i-\boldsymbol{g}\Delta t_{ij}\right)+\delta\boldsymbol{v}_{ij}, \\ & \Delta\tilde{p}_{ij}=R_{i}^{\top}\left(\boldsymbol{p}_{j}-\boldsymbol{p}_{i}-\boldsymbol{v}_{i}\Delta t_{ij}-\frac{1}{2}\boldsymbol{g}\Delta t_{ij}^{2}\right)+\delta\boldsymbol{p}_{ij}. \end{aligned}$

这个式子归纳了前面我们讨论的内容，显示了预积分的几大优点：

1. 它的左侧是可以通过 IMU 的测量值积分得到的预积分测量值，右侧是根据状态变量推断出来的预测值，再加上（或乘上）一个随机噪声。
2. 左侧变量的定义方式非常适合程序实现。 $\Delta\tilde{R}_{ik}$ 可以通过 $k$ 时刻 IMU 读数得到， $\Delta\tilde{v}_{ik}$ 可以由 $k$ 时刻 IMU 读数和 $\Delta\tilde{R}_{ik}$ 算得，而 $\Delta\tilde{p}_{ik}$ 又可以通过 $k$ 时刻 IMU 读数和 $\Delta\tilde{R}_{ik}$ 和 $\Delta\tilde{v}_{ik}$ 计算结果得到。另一方面，如果知道了 $k$ 时刻的预积分测量值，又很容易根据 $k+1$ 时刻 IMU 读数，计算出 $k+1$ 时刻的预积分测量值。这是由预积分测量值的累加/累乘定义方式决定的。
3. 从右侧看来，也很容易根据 $i$ 和 $j$ 时刻的状态变量来推测预积分测量值的大小，从而写出误差公式，形成最小二乘。

将上式变化可以得到预测公式。通过预测公式可以使用 $i$ 时刻到 $j$ 时刻的预积分测量值和 $i$ 时刻状态推测 $j$ 时刻状态：

$\begin{aligned} & R_j=R_i\Delta\tilde{R}_{ij}\mathrm{Exp}(-\delta\phi_{ij}), \\ & v_j=R_i(\Delta\tilde{v}_{ij}-\delta v_{ij})+v_i+g\Delta t_{ij}, \\ & p_j=R_i(\Delta\tilde{p}_{ij}-\delta p_{ij})+p_i+v_i\Delta t_{ij}+\frac{1}{2}g\Delta t_{ij}^2 \end{aligned}$

忽略测量噪声得：

$\begin{aligned} & R_{j}=R_{i}\Delta\tilde{R}_{ij}, \\ & v_j=R_i\Delta\tilde{v}_{ij}+v_i+g\Delta t_{ij}, \\ & p_j=R_i\Delta\tilde{p}_{ij}+p_i+v_i\Delta t_{ij}+\frac{1}{2}g\Delta t_{ij}^2. \end{aligned}$

接下来继续推导预积分的噪声模型。

3 预积分的噪声模型和协方差矩阵

由于噪声项的定义比较复杂，本节会使用同样的思路来处理各种噪声项。我们会将复杂的噪声项线性化，保留一阶项系数，然后推导线性模型下的协方差矩阵变化。这是一种非常常见的处理思路，对许多复杂模型都很有效。

3.1 旋转部分

旋转部分的测量噪声如下，其中 ${\eta}_{gd,j}$ 为 $j$ 时刻离散化后的 IMU 陀螺仪测量噪声：

$\begin{aligned} & \mathrm{Exp}(-\delta\phi_{ij})=\prod_{k=i}^{j-1}\mathrm{Exp}(-\Delta\tilde{R}_{k+1,j}^{\top}J_{r,k}\eta_{gd,k}\Delta t) \end{aligned}$

由上式可知，作为随机变量的 $\delta\phi_{ij}$ 只和随机变量 ${\eta}_{gd}$ 有关，而其他的都是确定的观测量。

对上式两侧取 $Log$ 得：

$\begin{aligned} & \delta\phi_{ij}=-\mathrm{Log}\left(\prod_{k=i}^{j-1}\mathrm{Exp}(-\Delta\tilde{R}_{k+1,j}^{\top}J_{r,k}\eta_{gd,k}\Delta t)\right) \end{aligned}$

我们将其变换为递推形式，即从 ${j-1}$ 时刻噪声推断得到 $j$ 时刻噪声：

$\begin{aligned} & \delta\phi_{ij}=-\mathrm{Log}\left(\prod_{k=i}^{j-1}\mathrm{Exp}(-\Delta\tilde{R}_{k+1,j}^{\top}J_{r,k}\eta_{gd,k}\Delta t)\right) ,\\& \approx\Delta\tilde{R}_{j-1,j}^{\top}\delta\phi_{i,j-1}+J_{r,j-1}\eta_{gd,j-1}\Delta t. \end{aligned}$

注意上式中 $\Delta\tilde{R}_{j-1,j}^{\top}=\mathrm{Exp}((\tilde{\boldsymbol{\omega}}_{j-1}-\boldsymbol{b}_{g,i})\Delta t)$ 。

3.2 速度部分

速度部分的测量噪声如下，其中 ${\eta}_{ad,j}$ 为 $j$ 时刻离散化后的 IMU 加速度计测量噪声：

我们将其变换为递推形式，即从 ${j-1}$ 时刻噪声推断得到 $j$ 时刻噪声：

$\begin{aligned} & \delta v_{ij}\approx\sum_{k=i}^{j-1}\left[-\Delta\tilde{R}_{ik}(\tilde{a}_{k}-b_{a,i})^{\wedge}\delta\phi_{ik}\Delta t+\Delta\tilde{R}_{ik}\eta_{ad,k}\Delta t\right], \\&=\delta v_{i,j-1}-\Delta\tilde{R}_{i,j-1}(\tilde{a}_{j-1}-b_{a,i})^{\wedge}\delta\phi_{i,j-1}\Delta t+\Delta\tilde{R}_{i,j-1}\eta_{ad,j-1}\Delta t.\end{aligned}$

3.3 平移部分

平移部分的测量噪声如下，其中 ${\eta}_{ad,j}$ 为 $j$ 时刻离散化后的 IMU 加速度计测量噪声：

我们将其变换为递推形式，即从 ${j-1}$ 时刻噪声推断得到 $j$ 时刻噪声：

$\begin{aligned} & \delta p_{ij}=\sum_{k=i}^{j-1}\left[\delta v_{ik}\Delta t-\frac{1}{2}\Delta\tilde{R}_{ik}\left(\tilde{a}_{k}-b_{a,i}\right)^{\wedge}\delta\phi_{ik}\Delta t^{2}+\frac{1}{2}\Delta\tilde{R}_{ik}\eta_{ad,k}\Delta t^{2}\right] \\& =\delta p_{i,j-1}+\delta v_{i,j-1}\Delta t-\frac{1}{2}\Delta\tilde{R}_{i,j-1}(\tilde{a}_{j-1}-b_{a,i})^{\wedge}\delta\phi_{i,j-1}\Delta t^{2}+\frac{1}{2}\Delta\tilde{R}_{i,j-1}\eta_{ad,j-1}\Delta t^{2}.\end{aligned}$

3.4 噪声项合并

为了方便表示，将 3 种噪声项的递推形式合并为一个。令 $\eta_{ik}$ 为预积分测量噪声、 $\eta_{d,j}$ 为 $j$ 时刻离散化后的 IMU 测量噪声：

$\eta_{ik}= \begin{bmatrix} \delta\phi_{ik} \\ \\ \delta v_{ik} \\ \\ \delta p_{ik} \end{bmatrix},$ $\eta_{d,j}= \begin{bmatrix} \eta_{gd,j} \\ \\ \eta_{ad,j} \end{bmatrix},$

那么，从 $\eta_{i,j-1}$ 到 $\eta_{i,j}$ 的递推形式为：

$\boldsymbol\eta_{i,j}=\boldsymbol A_{j-1}\boldsymbol\eta_{i,j-1}+ \boldsymbol B_{j-1}\boldsymbol\eta_{i,j-1}$

其中，系数矩阵 $\boldsymbol A_{j-1},\boldsymbol B_{j-1}$ 为：

$\begin{aligned} A_{j-1}= \begin{bmatrix} \Delta\tilde{R}_{j-1,j}^\top & 0 & 0 \\ -\Delta\tilde{R}_{i,j-1}(\tilde{a}_{j-1}-b_{a,i})^\wedge\Delta t & I & 0 \\ -\frac{1}{2}\Delta\tilde{R}_{i,j-1}(\tilde{a}_{j-1}-b_{a,i})^\wedge\Delta t^2 & \Delta tI & I \end{bmatrix},B_{j-1}= \begin{bmatrix} J_{r,j-1}\Delta t & 0 \\ 0 & \Delta\tilde{R}_{i,j-1}\Delta t \\ 0 & \frac{1}{2}\Delta\tilde{R}_{i,j-1}\Delta t^2 \end{bmatrix}. \end{aligned}$

如果我们以协方差的形式来记录噪声，那么每次增加 IMU 观测时，噪声应该呈现出逐渐增大的关系：

$\Sigma_{i,j}=A_{j-1}\Sigma_{i,j-1}A_{j-1}^\top+B_{j-1}\mathrm{Cov}(\eta_{d,j-1})B_{j-1}^\top$

这里的 $\boldsymbol A_{j-1}$ 阵接近单位矩阵 $I$ ，因此可以看成将预积分测量噪声累加起来。陀螺仪的噪声通过 $\boldsymbol B_{j-1}$ 矩阵进入到旋转的观测量中，而加速度计的噪声则主要进入速度与平移估计中。这种累加关系很容易在程序中实现出来。后面我们会在实验章节中看到它们的实现。注意，如果预积分定义的残差项顺序发生改变，我们也需要调整这里的系统矩阵行列关系以保持一致性。

4 零偏的更新

先前的讨论都假设了在 $i$ 时刻的 IMU 零偏固定不变，当然这都是为了方便后续的计算。然而在实际的图优化中，我们经常会对状态变量（优化变量）进行更新，导致零偏发生改变。理论上来讲，如果 IMU零偏发生了变化，预积分就应该重新计算，因为预积分的每一步都用到了 $i$ 时刻的 IMU 零偏。但是实际操作过程中，我们也可以选用一种取巧的做法：假设预积分的测量值是随零偏线性变化的，当零偏估计发生改变时，在原先预积分测量值的基础上使用线性模型进行修正得到新的预积分测量值。具体来说，我们把预积分测量值看成 $b_{g,i},b_{a,i}$ 的函数，那么，当 $b_{g,i},b_{a,i}$ 更新了 $\delta\boldsymbol b_{g,i},\delta\boldsymbol b_{a,i}$ 之后，预积分观测应作如下的修正，下式即为预积分测量值的修正公式：

$\begin{gathered} \Delta\tilde{\boldsymbol{R}}_{ij}(\boldsymbol{b}_{g,i}+\delta\boldsymbol{b}_{g,i})=\Delta\tilde{R}_{ij}(b_{g,i})\mathrm{Exp}\left(\frac{\partial\Delta\tilde{\boldsymbol{R}}_{ij}}{\partial\boldsymbol{b}_{g,i}}\delta\boldsymbol{b}_{g,i}\right), \\ \Delta\tilde{\boldsymbol{v}}_{ij}(b_{g,i}+\delta\boldsymbol{b}_{g,i},\boldsymbol{b}_{a,i}+\delta\boldsymbol{b}_{a,i})=\Delta\tilde{\boldsymbol{v}}_{ij}(\boldsymbol{b}_{g,i},\boldsymbol{b}_{a,i})+\frac{\partial\Delta\tilde{\boldsymbol{v}}_{ij}}{\partial b_{g,i}}\delta\boldsymbol{b}_{g,i}+\frac{\partial\Delta\tilde{\boldsymbol{v}}_{ij}}{\partial\boldsymbol{b}_{a,i}}\delta\boldsymbol{b}_{a,i}, \\ \Delta\tilde{p}_{ij}(\boldsymbol{b}_{g,i}+\delta\boldsymbol{b}_{g,i},\boldsymbol{b}_{a,i}+\delta\boldsymbol{b}_{a,i})=\Delta\tilde{p}_{ij}(\boldsymbol{b}_{g,i},\boldsymbol{b}_{a,i})+\frac{\partial\Delta\tilde{\boldsymbol{p}}_{ij}}{\partial\boldsymbol{b}_{g,i}}\delta\boldsymbol{b}_{g,i}+\frac{\partial\Delta\tilde{\boldsymbol{p}}_{ij}}{\partial\boldsymbol{b}_{a,i}}\delta\boldsymbol{b}_{a,i}. \end{gathered}$

只需求出上式中预积分观测值 $\Delta\tilde{\boldsymbol{R}}_{ij},\Delta\tilde{\boldsymbol{v}}_{ij},\Delta\tilde{\boldsymbol{p}}_{ij}$ 相对于 $b_{g,i},b_{a,i}$ 的雅可比矩阵即可在零偏更新时对预积分观测值进行修正。

预积分观测值 $\Delta\tilde{\boldsymbol{R}}_{ij},\Delta\tilde{\boldsymbol{v}}_{ij},\Delta\tilde{\boldsymbol{p}}_{ij}$ 相对于 $b_{g,i},b_{a,i}$ 的雅可比矩阵如下：

$\begin{aligned} & \frac{\partial\Delta\tilde{R}_{ij}}{\partial b_{g,i}}=-\sum_{k=i}^{j-1}\left[\Delta\tilde{R}_{k+1,j}^{\top}J_{r,k}\Delta t\right], \\ & \frac{\partial\Delta\tilde{v}_{ij}}{\partial b_{a,i}}=-\sum_{k=i}^{j-1}\Delta\tilde{R}_{ik}\Delta t, \\ & \frac{\partial\Delta\tilde{v}_{ij}}{\partial b_{g,i}}=-\sum_{k=i}^{j-1}\Delta\tilde{R}_{ik}\left(\tilde{a}_{k}-b_{a,i}\right)^{\wedge}\frac{\partial\Delta\tilde{R}_{ik}}{\partial b_{g,i}}\Delta t, \\ & \frac{\partial\Delta\tilde{p}_{ij}}{\partial b_{a,i}}=\sum_{k=i}^{j-1}\left[\frac{\partial\Delta\tilde{v}_{ik}}{\partial b_{a,i}}\Delta t-\frac{1}{2}\Delta\tilde{R}_{ik}\Delta t^{2}\right], \\ & \frac{\partial\Delta\tilde{p}_{ij}}{\partial b_{g,i}}=\sum_{k=i}^{j-1}\left[\frac{\partial\Delta\tilde{\boldsymbol{v}}_{ik}}{\partial\boldsymbol{b}_{g,i}}\Delta t-\frac{1}{2}\Delta\tilde{\boldsymbol{R}}_{ik}\left(\tilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{a,i}\right)^{\wedge}\frac{\partial\Delta\tilde{\boldsymbol{R}}_{ik}}{\partial\boldsymbol{b}_{g,i}}\Delta t^{2}\right]. \end{aligned}$

将其变换为递推形式，即从 ${j-1}$ 时刻雅可比矩阵推断得到 $j$ 时刻雅可比矩阵：

$\begin{aligned} & \frac{\partial\Delta R_{ij}}{\partial b_{g,i}}=\Delta\tilde{R}_{j-1,j}^{\top}\frac{\partial\Delta R_{i,j-1}}{\partial b_{g,i}}-J_{r,j-1}\Delta t, \\ & \frac{\partial\Delta\tilde{v}_{ij}}{\partial b_{a,i}}=\frac{\partial\Delta\tilde{v}_{i,j-1}}{\partial b_{a,i}}-\Delta\tilde{R}_{i,j-1}\Delta t, \\ & \frac{\partial\Delta\tilde{v}_{ij}}{\partial b_{g,i}}=\frac{\partial\Delta\tilde{v}_{i,j-1}}{\partial b_{g,i}}-\Delta\tilde{R}_{i,j-1}\left(\tilde{a}_{j-1}-b_{a,i}\right)^{\wedge}\frac{\partial\Delta\tilde{R}_{i,j-1}}{\partial b_{g,i}}\Delta t, \\ & \frac{\partial\Delta\tilde{p}_{ij}}{\partial b_{a,i}}=\frac{\partial\Delta\tilde{p}_{i,j-1}}{\partial b_{a,i}}+\frac{\partial\Delta\tilde{v}_{i,j-1}}{\partial b_{a,i}}\Delta t-\frac{1}{2}\Delta\tilde{R}_{i,j-1}\Delta t^{2}, \\ & \frac{\partial\Delta\tilde{p}_{ij}}{\partial b_{g,i}}=\frac{\partial\Delta\tilde{p}_{i,j-1}}{\partial b_{g,i}}+\frac{\partial\Delta\tilde{v}_{i,j-1}}{\partial b_{g,i}}\Delta t-\frac{1}{2}\Delta\tilde{R}_{i,j-1}\left(\tilde{a}_{j-1}-b_{a,i}\right)^{\wedge}\frac{\partial\Delta\tilde{R}_{i,j-1}}{\partial b_{g,i}}\Delta t^{2} \end{aligned}$

5 图优化中的预积分边

现在我们定义了预积分的测量模型，推导了它的噪声模型和协方差矩阵，并说明了随着零偏量更新，预积分观测值应该怎么更新。事实上，我们已经可以把预积分观测作为图优化的因子（Factor）或者边（Edge）了。

在 IMU 相关的应用中，通常把每个时刻的状态建模为包含旋转、平移、线速度、IMU 零偏的变量，构成状态变量集合 $\mathcal{X}$ ：

$x_k=[R,p,v,b_g,b_a]_k\in\mathcal{X}$

预积分模型构建了关键帧 $\boldsymbol i$ 与关键帧 $\boldsymbol j$ 之间的相对运动约束。

5.1 预积分边的残差

不同文献当中对残差的具体定义并不完全一致，残差的实际定义是相当灵活的。注意更换残差定义时，对应的噪声协方差可能会发生改变。

预积分本身的观测模型已经在预积分定义式中介绍，我们可以用 $i$ 时刻、 $j$ 时刻的状态变量值与预积分的观测量作差，得到残差的定义公式。

$\begin{aligned} & r_{\Delta R_{ij}}=\mathrm{Log}\left(\Delta\tilde{R}_{ij}^{\top}\left(R_{i}^{\top}R_{j}\right)\right), \\ & r_{\Delta v_{ij}}=R_{i}^{\top}(v_{j}-v_{i}-g\Delta t_{ij})-\Delta\tilde{v}_{ij}, \\ & r_{\Delta p_{ij}}=R_{i}^{\top}\left(p_{j}-p_{i}-v_{i}\Delta t_{ij}-\frac{1}{2}\boldsymbol{g}\Delta t_{ij}^{2}\right)-\Delta\tilde{\boldsymbol{p}}_{ij}. \end{aligned}$

通常我们会把 $r$ 统一写成一个 9 维的残差变量。它表面上关联两个时刻的旋转、平移、线速度，但由于预积分观测内部含有 IMU 零偏，所以实际也和 $i$ 时刻两个零偏有关。在优化过程中，如果对 $i$ 时刻的零偏进行更新，那么预积分观测量也应该线性地发生改变，从而影响残差项的取值。所以，尽管在这个残差项里似乎不含有 $b_{g,i},b_{a,i}$ ，它们显然是和残差相关的。因此，如果我们把预积分残差看作同一个，那么它与状态顶点的关联应该如下图所示。除了预积分因子本身之外，还需要约束 IMU 的随机游走，因此在 IMU 的不同时刻，零偏会存在一个约束因子。

关于图优化中顶点的形式，有以下两种：

把所有状态变量放在同一个顶点
选择“散装的形式”，即对旋转、平移、线速度和两个零偏分别构造顶点，共构造 5 个顶点，然后求解这几个顶点之间的雅可比。如果采用这种做法，那么雅可比矩阵的数量会变多，但单个雅可比矩阵的维度可以降低（单个雅可比通常为 3 × 3，而预积分观测量对状态变量的雅可比会变为 9 × 15，且有很多零块）。由于视觉和激光的观测约束通常只和 $R,t$ 相关，如果需要构造视觉和激光雷达的紧耦合 LIO 系统，则可以避免雅可比矩阵当中的一些零矩阵块。

总而言之，两种做法各有各的好处，本书的代码实现部分使用了散装做法，即把各状态量写成单独的顶点，分别来约束它们。

5.2 预积分残差相对于状态变量的雅可比矩阵

最后我们来讨论预积分残差相对状态变量的雅可比矩阵。

5.2.1 旋转部分

旋转部分的残差如下，其与状态变量 $R_i,R_j,b_{g,i}$ 有关：

$r_{\Delta R_{ij}}=\mathrm{Log}\left(\Delta\tilde{R}_{ij}^{\top}\left(R_{i}^{\top}R_{j}\right)\right)$

$\begin{aligned} & \Delta\tilde{\boldsymbol{R}}_{ij}=\prod_{k=i}^{j-1}\operatorname{Exp}\left((\tilde{\boldsymbol{\omega}}_{k}-\boldsymbol{b}_{g,i})\Delta t\right) \end{aligned}$

假设优化初始的零偏为 $\boldsymbol{b}_{g,i}$ ，在某一步迭代时，我们当前估计出来的零偏修正为 $\delta\boldsymbol{b}_{g,i}$ ，而当前修正得到的预积分旋转观测量为 $\Delta\tilde{R}_{ij}^{\prime}=\Delta\tilde{R}_{ij}\left(b_{g,i}+\delta b_{g,i}\right)$ ，残差为 $r_{\Delta R_{ij}}^{\prime}$ 。预积分旋转残差相对状态变量的雅可比矩阵如下：

$\begin{aligned} & \frac{\partial r_{\Delta R_{ij}}}{\partial R_i}=-J_r^{-1}(r_{\Delta R_{ij}})R_j^\top R_i, \\ & \frac{\partial r_{\Delta R_{ij}}}{\partial R_j}=J_r^{-1}(r_{\Delta R_{ij}}), \\& \frac{\partial r_{\Delta R_{ij}}}{\partial b_{g,i}}=-J_{r}^{-1}(r_{\Delta R_{ij}}^{\prime})\mathrm{Exp}\left(r_{\Delta R_{ij}}^{\prime}\right)^{\top}\boldsymbol{J}_{r,b}\frac{\partial\Delta\tilde{\boldsymbol{R}}_{ij}}{\partial\boldsymbol{b}_{g,i}}. \end{aligned}$

上式中的 $\mathrm{Exp}\left(r_{\Delta R_{ij}}^{\prime}\right)^{\top}$ 为 $\left[\Delta\tilde{R}_{ij}^{\top}\left(R_{i}^{\top}R_{j}\right)\right]^{-1}$ ， $\boldsymbol{J}_{r,b}$ 为 $\boldsymbol{J}_{r}(\frac{\partial\Delta\tilde{\boldsymbol{R}}_{ij}}{\partial\boldsymbol{b}_{g,i}}\delta\boldsymbol{b}_{g,i})$ 。

5.2.2 速度部分

速度部分的残差如下，其与状态变量 $R_i,v_i,v_j,b_{g,i},b_{a,i}$ 有关：

$r_{\Delta v_{ij}}=R_{i}^{\top}(v_{j}-v_{i}-g\Delta t_{ij})-\Delta\tilde{v}_{ij}$

$\begin{aligned} & \Delta\tilde{\boldsymbol{v}}_{ij}=\sum_{k=i}^{j-1}\Delta\tilde{\boldsymbol{R}}_{ik}(\tilde{\boldsymbol{a}}_{k}-\boldsymbol{b}_{a,i})\Delta t \end{aligned}$

预积分速度残差相对状态变量的雅可比矩阵如下：

$\begin{aligned} &\frac{\partial r_{\Delta v_{ij}}}{\partial R_i}=\left(R_i^\top(v_j-v_i-g\Delta t_{ij})\right)^\wedge, \\ & \frac{\partial r_{\Delta\boldsymbol{v}_{ij}}}{\partial\boldsymbol{v}_{i}}=-R_{i}^{\top}, \\ & \frac{\partial\boldsymbol{r}_{\Delta\boldsymbol{v}_{ij}}}{\partial\boldsymbol{v}_{j}}=R_{i}^{\top}, \\ & \frac{\partial r_{\Delta v_{ij}}}{\partial b_{g,i}}=-\frac{\partial\Delta\tilde{v}_{ij}}{\partial b_{g,i}}, \\ & \frac{\partial r_{\Delta v_{ij}}}{\partial b_{a,i}}=-\frac{\partial\Delta\tilde{v}_{ij}}{\partial b_{a,i}}.\end{aligned}$

5.2.3 平移部分

平移部分的残差如下，其与状态变量 $R_i,v_i,p_i,p_j,b_{g,i},b_{a,i}$ 有关：

$r_{\Delta p_{ij}}=R_{i}^{\top}\left(p_{j}-p_{i}-v_{i}\Delta t_{ij}-\frac{1}{2}\boldsymbol{g}\Delta t_{ij}^{2}\right)-\Delta\tilde{\boldsymbol{p}}_{ij}$

预积分平移残差相对状态变量的雅可比矩阵如下：

$\begin{aligned} & \frac{\partial\boldsymbol{r}_{\Delta\boldsymbol{p}_{ij}}}{\partial\phi_i} =\left(R_{i}^{\top}\left(p_{j}-p_{i}-v_{i}\Delta t_{ij}-\frac{1}{2}g\Delta t_{ij}^{2}\right)\right)^{\wedge}, \\& \frac{\partial\boldsymbol{r}_{\Delta\boldsymbol{p}_{ij}}}{\partial\boldsymbol{v}_{i}}=-R_{i}^{\top}\Delta t_{ij}, \\& \frac{\partial\boldsymbol{r}_{\Delta\boldsymbol{p}_{ij}}}{\partial\boldsymbol{p}_{i}} =-R_i^{\top}, \\ & \frac{\partial r_{\Delta p_{ij}}}{\partial p_j} =R_i^\top, \\ & \frac{\partial r_{\Delta p_{ij}}}{\partial b_{g,i}}=-\frac{\partial\Delta\tilde{p}_{ij}}{\partial b_{g,i}}, \\ & \frac{\partial r_{\Delta p_{ij}}}{\partial b_{a,i}}=-\frac{\partial\Delta\tilde{p}_{ij}}{\partial b_{a,i}}. \end{aligned}$

6 预积分流程

在一个关键帧组成的系统中，我们可以从任意一个时刻的关键帧出发开始预积分，并且在任一时刻停止预积分过程。之后，我们可以把预积分的观测量、噪声以及各种累计雅可比取出来，用于约束两个关键帧的状态。按照先前的讨论，在开始预积分之后，当 $j-1$ 时刻一个新的 IMU 数据到来时，我们的程序应该完成以下任务：

1. 在上一个数据基础上，利用下式，计算三个预积分观测量（其中 $\Delta\tilde{\boldsymbol{R}}_{ij}$ 右乘更新）： $\Delta\tilde{\boldsymbol{R}}_{ij},\Delta\tilde{\boldsymbol{v}}_{ij},\Delta\tilde{\boldsymbol{p}}_{ij}$ ；

$\begin{aligned} & \Delta\tilde{\boldsymbol{R}}_{ij}=\Delta\tilde{\boldsymbol{R}}_{i,j-1}*\mathrm{Exp}((\tilde{\boldsymbol{\omega}}_{j-1}-\boldsymbol{b}_{g,i})\Delta t) \\ & \Delta\tilde{\boldsymbol{v}}_{ij}=\Delta\tilde{\boldsymbol{v}}_{i,j-1}+\Delta\tilde{\boldsymbol{R}}_{ij}(\tilde{\boldsymbol{a}}_{j-1}-\boldsymbol{b}_{a,i})\Delta t \\ & \Delta\tilde{p}_{ij}=\Delta\tilde{p}_{i,j-1}+\left[(\Delta\tilde{\boldsymbol{v}}_{i,j-1}\Delta t)+\frac{1}{2}\Delta\tilde{\boldsymbol{R}}_{i,j-1}(\tilde{\boldsymbol{a}}_{j-1}-\boldsymbol{b}_{a,i})\Delta t^{2}\right] \end{aligned}$

2. 计算三个噪声量的协方差矩阵，作为后续图优化的信息矩阵；

$\eta_{ij}= \begin{bmatrix} \delta\phi_{ij} \\ \\ \delta v_{ij} \\ \\ \delta p_{ij} \end{bmatrix},$ $\eta_{d,j}= \begin{bmatrix} \eta_{gd,j} \\ \\ \eta_{ad,j} \end{bmatrix},$

$\Sigma_{i,j}=A_{j-1}\Sigma_{i,j-1}A_{j-1}^\top+B_{j-1}\mathrm{Cov}(\eta_{d,j-1})B_{j-1}^\top$

其中 $\Delta\tilde{R}_{j-1,j}^{\top}=\mathrm{Exp}((\tilde{\boldsymbol{\omega}}_{j-1}-\boldsymbol{b}_{g,i})\Delta t)$ ， $\boldsymbol{J}_{r,j-1}=\boldsymbol{J}_{r}[(\tilde{\boldsymbol{\omega}}_{j-1}-\boldsymbol{b}_{g,i})\Delta t]$ 。

3. 计算预积分观测量相对于零偏的雅可比矩阵，当零偏更新时进行预积分测量值的修正；

4.增量积分时间 $\Delta t_{ij}$ 。
5.可以使用 $i$ 时刻到 $j$ 时刻的预积分测量值 $\Delta\tilde{\boldsymbol{R}}_{ij},\Delta\tilde{\boldsymbol{v}}_{ij},\Delta\tilde{\boldsymbol{p}}_{ij}$ 推测 $j$ 时刻状态；

7 预积分的程序实现

7.1 程序实现

一个预积分类应该存储以下数据：

• 预积分的观测量 $\Delta\tilde{\boldsymbol{R}}_{ij},\Delta\tilde{\boldsymbol{v}}_{ij},\Delta\tilde{\boldsymbol{p}}_{ij}$ ；
• 预积分开始时的 IMU 零偏 $b_g$ , $b_a$ ；
• 在积分时期内的测量噪声 $\Sigma_{i,j}$ 。
• 各积分量对 IMU 零偏的雅可比矩阵。
• 整个积分时间 $\Delta t_{ij}$ 。

/**
 * IMU 预积分器
 *
 * 调用Integrate来插入新的IMU读数，然后通过Get函数得到预积分的值
 * 雅可比也可以通过本类获得，可用于构建g2o的边类
 */
class IMUPreintegration {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    /// 参数配置项
    /// 初始的零偏需要设置，其他可以不改
    struct Options {
        Options() {}
        Vec3d init_bg_ = Vec3d::Zero();  // 初始零偏
        Vec3d init_ba_ = Vec3d::Zero();  // 初始零偏
        double noise_gyro_ = 1e-2;       // 陀螺噪声，标准差
        double noise_acce_ = 1e-1;       // 加计噪声，标准差
    };


   public:
    double dt_ = 0;                          // 整体预积分时间
    Mat9d cov_ = Mat9d::Zero();              // 累计噪声矩阵
    Mat6d noise_gyro_acce_ = Mat6d::Zero();  // 测量噪声矩阵

    // 零偏
    Vec3d bg_ = Vec3d::Zero();
    Vec3d ba_ = Vec3d::Zero();

    // 预积分观测量
    SO3 dR_;
    Vec3d dv_ = Vec3d::Zero();
    Vec3d dp_ = Vec3d::Zero();

    // 雅可比矩阵
    Mat3d dR_dbg_ = Mat3d::Zero();
    Mat3d dV_dbg_ = Mat3d::Zero();
    Mat3d dV_dba_ = Mat3d::Zero();
    Mat3d dP_dbg_ = Mat3d::Zero();
    Mat3d dP_dba_ = Mat3d::Zero();
};

注意 IMU 零偏相关的噪声项并不直接和预积分类有关，我们将它们挪到优化类当中。

单个 IMU 的积分函数实现如下：

void IMUPreintegration::Integrate(const IMU &imu, double dt) {
    // 去掉零偏的测量
    Vec3d gyr = imu.gyro_ - bg_;  // 陀螺
    Vec3d acc = imu.acce_ - ba_;  // 加计

    // 更新dv, dp, 见p105 (4.9), (4.13), (4.16) 
    // 等号右侧 dp_ dv_ 为 i 时刻预积分观测量， 等号左侧 dp_ dv_ 为 j 时刻预积分观测量
    Vec3d omega = gyr * dt;         // 转动量
    SO3 deltaR = SO3::exp(omega);   // exp后
    
    SO3 new_dR = dR_ * deltaR;
    Vec3d new_dv = dv_ + dR_ * acc * dt;
    Vec3d new_dp = dp_ + dv_ * dt + 0.5f * dR_.matrix() * acc * dt * dt;

    Mat3d rightJ = SO3::jr(omega);  // 右雅可比
    Mat3d acc_hat = SO3::hat(acc);
    double dt2 = dt * dt;

    // 运动方程雅可比矩阵系数，A,B阵，见p108 (4.29)
    Eigen::Matrix<double, 9, 9> A;
    A.setIdentity();
    Eigen::Matrix<double, 9, 6> B;
    B.setZero();

    A.block<3, 3>(0, 0) = deltaR.matrix().transpose(); // 
    A.block<3, 3>(3, 0) = -dR_.matrix() * dt * acc_hat;
    A.block<3, 3>(6, 0) = -0.5f * dR_.matrix() * acc_hat * dt2;
    A.block<3, 3>(6, 3) = dt * Mat3d::Identity();

    B.block<3, 3>(0, 0) = rightJ * dt;
    B.block<3, 3>(3, 3) = dR_.matrix() * dt;
    B.block<3, 3>(6, 3) = 0.5f * dR_.matrix() * dt2;

    // 更新噪声项
    cov_ = A * cov_ * A.transpose() + B * noise_gyro_acce_ * B.transpose();  

    // 更新各雅可比，见式p111 (4.39)
    dR_dbg_ = deltaR.matrix().transpose() * dR_dbg_ - rightJ * dt;                     // p111 (4.39a)
    dV_dba_ = dV_dba_ - dR_.matrix() * dt;                                             // (4.39b)
    dV_dbg_ = dV_dbg_ - dR_.matrix() * dt * acc_hat * dR_dbg_;                         // (4.39c)
    dP_dba_ = dP_dba_ + dV_dba_ * dt - 0.5f * dR_.matrix() * dt2;                      // (4.39d)
    dP_dbg_ = dP_dbg_ + dV_dbg_ * dt - 0.5f * dR_.matrix() * dt2 * acc_hat * dR_dbg_;  // (4.39e)

    // 更新预积分测量值
    dR_ = new_dR; 
    dv_ = new_dv; 
    dp_ = new_dp;  

    // my 增量积分时间
    dt_ += dt;
}

注意：如果不进行优化，预积分和直接积分的效果是完全一致的，都是将 IMU 的数据积分起来。

在预积分之后，我们也可以像 ESKF 一样，从起始状态向最终状态进行预测。预测函数实现是非常简单的：

NavStated IMUPreintegration::Predict(const sad::NavStated &start, const Vec3d &grav) const {
    // grav 存在默认值
    // std::cout << "grav: " << grav.transpose() << std::endl;
    // p105 (4.18) 变形版， 忽略噪声后 i 时刻状态和 j 时刻状态的关系式, 这里的 dt_ 是整体预积分时间
    SO3 Rj = start.R_ * dR_;
    Vec3d vj = start.R_ * dv_ + start.v_ + grav * dt_;
    // pj 有点问题
    Vec3d pj = start.R_ * dp_ + start.p_ + start.v_ * dt_ + 0.5f * grav * dt_ * dt_;

    auto state = NavStated(start.timestamp_ + dt_, Rj, pj, vj);
    state.bg_ = bg_;
    state.ba_ = ba_;
    return state;
}

与 ESKF 不同的是：

预积分可以对多个 IMU 数据进行预测，可以从任意起始时刻向后预测
ESKF 通常只在当前状态下，针对单个 IMU 数据，向下一个时刻预测

7.2 测试

1 测试在恒定角速度运转下的预积分情况

TEST(PREINTEGRATION_TEST, ROTATION_TEST) {
    // 测试在恒定角速度运转下的预积分情况
    double imu_time_span = 0.01;       // IMU测量间隔
    Vec3d constant_omega(0, 0, M_PI);  // 角速度为180度/s，转1秒应该等于转180度
    Vec3d gravity(0, 0, -9.8);         // Z 向上，重力方向为负

    sad::NavStated start_status(0), end_status(1.0);
    sad::IMUPreintegration pre_integ;

    // 对比直接积分
    Sophus::SO3d R;
    Vec3d t = Vec3d::Zero();
    Vec3d v = Vec3d::Zero();

    for (int i = 1; i <= 100; ++i) {
        double time = imu_time_span * i;
        Vec3d acce = -gravity;  // 水平放置时加速度计应该测量到一个向上的力，为 -g
        pre_integ.Integrate(sad::IMU(time, constant_omega, acce), imu_time_span);

        sad::NavStated this_status = pre_integ.Predict(start_status, gravity);

        // p101 (4.4)
        t = t + v * imu_time_span + 0.5 * gravity * imu_time_span * imu_time_span +
            0.5 * (R * acce) * imu_time_span * imu_time_span;
        v = v + gravity * imu_time_span + (R * acce) * imu_time_span;
        R = R * Sophus::SO3d::exp(constant_omega * imu_time_span);

        // 验证在简单情况下，直接积分和预积分结果相等
        // Google Test框架中的断言（assertions），用于比较预期值和实际值是否在允许的误差范围内。如果不满足条件，则报 Failure
        EXPECT_NEAR(t[0], this_status.p_[0], 1e-2);
        EXPECT_NEAR(t[1], this_status.p_[1], 1e-2);
        EXPECT_NEAR(t[2], this_status.p_[2], 1e-2);

        EXPECT_NEAR(v[0], this_status.v_[0], 1e-2);
        EXPECT_NEAR(v[1], this_status.v_[1], 1e-2);
        EXPECT_NEAR(v[2], this_status.v_[2], 1e-2);

        EXPECT_NEAR(R.unit_quaternion().x(), this_status.R_.unit_quaternion().x(), 1e-4);
        EXPECT_NEAR(R.unit_quaternion().y(), this_status.R_.unit_quaternion().y(), 1e-4);
        EXPECT_NEAR(R.unit_quaternion().z(), this_status.R_.unit_quaternion().z(), 1e-4);
        EXPECT_NEAR(R.unit_quaternion().w(), this_status.R_.unit_quaternion().w(), 1e-4);
    }

    end_status = pre_integ.Predict(start_status, gravity);

    LOG(INFO) << "preinteg result: ";
    LOG(INFO) << "end rotation: \n" << end_status.R_.matrix();
    LOG(INFO) << "end trans: \n" << end_status.p_.transpose();
    LOG(INFO) << "end v: \n" << end_status.v_.transpose();

    LOG(INFO) << "direct integ result: ";
    LOG(INFO) << "end rotation: \n" << R.matrix();
    LOG(INFO) << "end trans: \n" << t.transpose();
    LOG(INFO) << "end v: \n" << v.transpose();
    SUCCEED();
}

测试结果：

./test_preintegration --gtest_filter=PREINTEGRATION_TEST.ROTATION_TEST

Note: Google Test filter = PREINTEGRATION_TEST.ROTATION_TEST
[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from PREINTEGRATION_TEST
[ RUN      ] PREINTEGRATION_TEST.ROTATION_TEST
I0116 22:22:12.217562 490210 test_preintegration.cc:74] preinteg result: 
I0116 22:22:12.217624 490210 test_preintegration.cc:75] end rotation: 
000000000-1 03.1225e-16 00000000000
-3.1225e-16 000000000-1 00000000000
00000000000 00000000000 00000000001
I0116 22:22:12.217639 490210 test_preintegration.cc:76] end trans: 
000000000000 000000000000 -4.44089e-15
I0116 22:22:12.217643 490210 test_preintegration.cc:77] end v: 
000000000000 000000000000 -1.77636e-15
I0116 22:22:12.217648 490210 test_preintegration.cc:79] direct integ result: 
I0116 22:22:12.217648 490210 test_preintegration.cc:80] end rotation: 
000000000-1 03.1225e-16 00000000000
-3.1225e-16 000000000-1 00000000000
00000000000 00000000000 00000000001
I0116 22:22:12.217655 490210 test_preintegration.cc:81] end trans: 
0 0 0
I0116 22:22:12.217658 490210 test_preintegration.cc:82] end v: 
0 0 0
[       OK ] PREINTEGRATION_TEST.ROTATION_TEST (0 ms)
[----------] 1 test from PREINTEGRATION_TEST (0 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (0 ms total)
[  PASSED  ] 1 test.

2. 测试在恒定加速度运行下的预积分情况

TEST(PREINTEGRATION_TEST, ACCELERATION_TEST) {
    // 测试在恒定加速度运行下的预积分情况
    double imu_time_span = 0.01;     // IMU测量间隔
    Vec3d gravity(0, 0, -9.8);       // Z 向上，重力方向为负
    Vec3d constant_acce(0.1, 0, 0);  // x 方向上的恒定加速度

    sad::NavStated start_status(0), end_status(1.0);
    sad::IMUPreintegration pre_integ;

    // 对比直接积分
    Sophus::SO3d R;
    Vec3d t = Vec3d::Zero();
    Vec3d v = Vec3d::Zero();

    for (int i = 1; i <= 100; ++i) {
        double time = imu_time_span * i;
        Vec3d acce = constant_acce - gravity;
        pre_integ.Integrate(sad::IMU(time, Vec3d::Zero(), acce), imu_time_span);
        sad::NavStated this_status = pre_integ.Predict(start_status, gravity);

        t = t + v * imu_time_span + 0.5 * gravity * imu_time_span * imu_time_span +
            0.5 * (R * acce) * imu_time_span * imu_time_span;
        v = v + gravity * imu_time_span + (R * acce) * imu_time_span;

        // 验证在简单情况下，直接积分和预积分结果相等
        EXPECT_NEAR(t[0], this_status.p_[0], 1e-2);
        EXPECT_NEAR(t[1], this_status.p_[1], 1e-2);
        EXPECT_NEAR(t[2], this_status.p_[2], 1e-2);

        EXPECT_NEAR(v[0], this_status.v_[0], 1e-2);
        EXPECT_NEAR(v[1], this_status.v_[1], 1e-2);
        EXPECT_NEAR(v[2], this_status.v_[2], 1e-2);

        EXPECT_NEAR(R.unit_quaternion().x(), this_status.R_.unit_quaternion().x(), 1e-4);
        EXPECT_NEAR(R.unit_quaternion().y(), this_status.R_.unit_quaternion().y(), 1e-4);
        EXPECT_NEAR(R.unit_quaternion().z(), this_status.R_.unit_quaternion().z(), 1e-4);
        EXPECT_NEAR(R.unit_quaternion().w(), this_status.R_.unit_quaternion().w(), 1e-4);
    }

    end_status = pre_integ.Predict(start_status, gravity);
    LOG(INFO) << "preinteg result: ";
    LOG(INFO) << "end rotation: \n" << end_status.R_.matrix();
    LOG(INFO) << "end trans: \n" << end_status.p_.transpose();
    LOG(INFO) << "end v: \n" << end_status.v_.transpose();

    LOG(INFO) << "direct integ result: ";
    LOG(INFO) << "end rotation: \n" << R.matrix();
    LOG(INFO) << "end trans: \n" << t.transpose();
    LOG(INFO) << "end v: \n" << v.transpose();
    SUCCEED();
}

测试结果：

./test_preintegration --gtest_filter=PREINTEGRATION_TEST.ACCELERATION_TEST

Note: Google Test filter = PREINTEGRATION_TEST.ACCELERATION_TEST
[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from PREINTEGRATION_TEST
[ RUN      ] PREINTEGRATION_TEST.ACCELERATION_TEST
I0116 22:26:44.789176 490325 test_preintegration.cc:126] preinteg result: 
I0116 22:26:44.789251 490325 test_preintegration.cc:127] end rotation: 
1 0 0
0 1 0
0 0 1
I0116 22:26:44.789265 490325 test_preintegration.cc:128] end trans: 
000000000.05 000000000000 -4.44089e-15
I0116 22:26:44.789271 490325 test_preintegration.cc:129] end v: 
0000000000.1 000000000000 -1.77636e-15
I0116 22:26:44.789274 490325 test_preintegration.cc:131] direct integ result: 
I0116 22:26:44.789276 490325 test_preintegration.cc:132] end rotation: 
1 0 0
0 1 0
0 0 1
I0116 22:26:44.789283 490325 test_preintegration.cc:133] end trans: 
0.05 0000 0000
I0116 22:26:44.789286 490325 test_preintegration.cc:134] end v: 
0.1 000 000
[       OK ] PREINTEGRATION_TEST.ACCELERATION_TEST (0 ms)
[----------] 1 test from PREINTEGRATION_TEST (0 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (0 ms total)
[  PASSED  ] 1 test.

查看全文

http://www.kler.cn/a/507085.html