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

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

news 来源：原创 2025/6/8 2:46:07

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;  // 水平放置时加速度计应该测量到一个向上的力，为 -gpre_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），用于比较预期值和实际值是否在允许的误差范围内。如果不满足条件，则报 FailureEXPECT_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_TESTNote: 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_TESTNote: 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.

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

目录 1 预积分的定义 2 预积分的测量模型 ( 预积分的测量值可由 IMU 的测量值积分得到 ) 2.1 旋转部分 2.2 速度部分 2.3 平移部分 2.4 将预积分测量和误差式代回最初的定义式 3 预积分的噪声模型和协方差矩阵 3.1 旋转部分 3.2 速度部分 3.3 平移部分 3.4 噪声项合并 4 零偏的…...

编程日记 2025/5/29 20:33:51

海量数据的处理

一般来说都是针对数据量特别大，内存有限制的。第一类：topk问题比如，在海量数据中找前50大的数据怎么办？ 方法一：使用小顶堆，用小顶堆维护这50个元素，当有新元素到来时，直接与堆…...

编程日记 2025/6/4 3:37:38

Python人脸识别库DeepFace使用教程及源码解析

目录一、DeepFace介绍 1、人脸库设计 2、DeepFace.find 3、DeepFace.verify 4、DeepFace.analyze 5、DeepFace.extract_faces 6、DeepFace.represent 7、DeepFace.stream 二、DeepFace二次开发 1、开发活体检测API 2、模型权重持久化三、总结一、DeepFace介绍 …...

编程日记 2025/6/3 16:48:03

Nacos：使用PgSQL数据源

数据源插件开源仓库地址：nacos-datasource-extend-plugins 一、PostgreSQL数据库安装 1、本文使用Docker进行数据库的安装，使用docker命令拉取的PG14版本的数据库： docker pull postgres:14.6 2、创建PG容器并启动，映射了5432…...

编程日记 2025/6/2 10:16:32

基于Python的多元医疗知识图谱构建与应用研究（下）

五、基于医疗知识图谱的医疗知识图谱程序构建 5.1 数据层构建 5.1.1 数据源选择与获取在构建基于医疗知识图谱的医疗知识图谱数据层时，数据源的选择与获取至关重要。数据源的质量和丰富度直接决定了知识图谱的可靠性和实用性。医学文献是重要的数据源之一，包括学术期刊论…...

编程日记 2025/6/2 7:07:17

JAVA：Spring Boot 实现责任链模式处理订单流程的技术指南

1、简述在复杂的业务系统中，订单流程往往需要一系列的操作，比如验证订单、检查库存、处理支付、更新订单状态等。责任链模式（Chain of Responsibility）可以帮助我们将这些处理步骤分开，并且以链式方式处理每一个操作…...

编程日记 2025/6/3 1:39:45

SpringBoot多级配置文件

1.问题先导有这样的场景，我们开发完毕后需要测试人员进行测试，由于测试环境和开发环境的很多配置都不相同，所以测试人员在运行我们的工程时需要临时修改很多配置，如下 java –jar springboot.jar –-spring.profiles.activete…...

编程日记 2025/6/2 7:07:12

阿里云安装mikrotik7配置内网互通

阿里云近期推出了200M不限量机器，对于没有公网接入的中小企业可以借助这个机器对多地分支机构进行内网互通。目前已经有很多机构用这个搞跨云k8s,跨云集群了。 mikrotik作为一个商用的软件，操作性比一些开源的软件好用不少。本文使用的网段为172.16.1…...

编程日记 2025/6/3 1:39:12

std::forward实现原理与应用场景

std::forward 是 C11 引入的一个函数模板，用于实现完美转发（Perfect Forwarding）。它的核心作用是根据传入的参数，决定将参数以左值引用还是右值引用的方式进行转发，从而保持参数的原始值类别。实现原理 template&l…...

编程日记 2025/6/2 7:07:08

TiDB 在市面上的热门应用领域

TiDB 在市面上的热门应用领域 TiDB 作为一款分布式数据库，凭借其高可扩展性和强一致性，逐渐成为多个行业和领域的热门选择。那么，TiDB 在市面上主要应用在哪些领域呢？今天我们来看看 TiDB 在几个热门领域的应用场景。 1. 互联网…...

编程日记 2025/6/3 5:13:29

“深入浅出”系列之C++：（11）推荐一些C++的开源项目

1. SQLiteCpp - 简单易用的Sqlite C封装库仓库地址：https://github.com/SRombauts/SQLiteCpp 简介：SQLiteCpp是一个对Sqlite数据库进行C封装的开源库，代码行数约2,500行。它提供了简洁易用的接口，使得在C项目中操作Sqlite数据库…...

编程日记 2025/6/4 1:13:23

高等数学学习笔记 ☞ 定积分的积分方法

1. 定积分的换元积分法 1. 换元积分公式：设函数在闭区间上连续，令，若满足： ①：当时，；当时，。此时的大小关系不一定，但与最好对应着写，否则就要留意变号的问…...

编程日记 2025/6/4 8:48:40

KVA教程-插件开发

“如果结果不如你所愿，就在尘埃落定前奋力一搏。”——《夏目友人帐》 “有些事不是看到了希望才去坚持，而是因为坚持才会看到希望。”——《十宗罪》 “维持现状意味着空耗你的努力和生命。”——纪伯伦 KVA 技术教程 * 插件开发简介插件开发是KVA&a…...

编程日记 2025/6/3 1:38:08

AI守护煤矿安全生产：基于视频智能的煤矿管理系统架构解析

前言本文我将介绍我和我的团队自主研发设计的一款AI产品的成果展示——“基于视频AI识别技术的煤矿安全生产管理系统”。这款产品是目前我在创业阶段和几位矿业大学的博士共同从架构设计、开发到交付的全过程中首次在博客频道发布, 我之前一直想写但没有机会来整理这套系统的…...

编程日记 2025/6/3 0:41:18

AI编程工具横向评测--Cloudstudio塑造完全态的jupyter notebook助力数据分析应用开发

AI编程工具横向评测–Cloudstudio塑造完全态的jupyter notebook助力数据分析应用开发数据分析类应用的开发，指的是首先进行数据分析，比如统计学分析、机器学习模型的构建等，然后将分析的流程开发成数据分析类的工具，或者将数据分…...

编程日记 2025/6/3 21:49:34

04JavaWeb——Maven-SpringBootWeb入门

Maven 课程内容初识Maven Maven概述 Maven模型介绍 Maven仓库介绍 Maven安装与配置 IDEA集成Maven 依赖管理 01. Maven课程介绍 1.1 课程安排学习完前端Web开发技术后，我们即将开始学习后端Web开发技术。做为一名Java开发工程师，后端Web开发…...

编程日记 2025/6/3 21:52:29

ThreeJS能力演示——界面点选交互能力

1、支持界面点选点选模型整体思路是：根据camera位置作为起始点，叠加鼠标相对位置作为偏置，摄像头方向作为射线方向。根据射线方向中的遇到的3D物体列表，第一个遇到的物体作为被点选的物体。 // 鼠标事件处理let selectedObjec…...

编程日记 2025/6/3 19:30:56

Linux：常用命令--文件与目录操作

ls命令功能：（list）列出当前目录的文件信息语法：ls [-l -h -a] [参数] 参数：被查看的文件夹，不提供参数，表示查看当前工作目录-l，以列表形式查看每个文件的属性，包含…...

编程日记 2025/6/3 21:52:12

Node.js NativeAddon 构建工具：node-gyp 安装与配置完全指南

Node.js NativeAddon 构建工具：node-gyp 安装与配置完全指南 node-gyp Node.js native addon build tool [这里是图片001] 项目地址: https://gitcode.com/gh_mirrors/no/node-gyp 项目基础介绍及主要编程语言 Node.js NativeAddon 构建工具（node-gyp…...

编程日记 2025/6/3 21:50:09

docker运行Java项目，Kaptcha因为字体缺失没法显示验证码图片

2015工作至今，10年资深全栈工程师，CTO，擅长带团队、攻克各种技术难题、研发各类软件产品，我的代码态度：代码虐我千百遍，我待代码如初恋，我的工作态度：极致，责任&#xff…...

编程日记 2025/6/3 0:21:57

C++otlv4连接sql serveer使用记录（注意点）

C使用otlv4在做插入时，有一些设计的坑需要注意插入数据： 当要给表中插入单个字符时，数据库表设计使用varchar(1)是合理的，但是otlv4一直报错char。后续查很久才知道，otlv4所写的绑定的字符数组的长度应该实际数组…...

编程日记 2025/6/3 21:51:56

[思考记录]认知和思考

在以前，具备一定的技能和经验就能轻易找到自己的一席之地。但在AI时代下，这些东西很容易就被抹平，那么我们的竞争力又在哪里？“认知和思考”是一个方向，帮助我们能去应对复杂情境、帮我们更容易去看到真相。 1.很多时…...

编程日记 2025/6/3 21:50:48

前端开发Web

Ajax 概念:Asynchronous JavaScriptAnd XML，异步的JavaScript和XML 作用: 数据交换:通过Ajax可以给服务器发送请求，并获取服务器响应的数据。异步交互:可以在不重新加载整个页面的情况下，与服务器交换数据并更新部分网页的…...

编程日记 2025/6/3 5:07:47

【C++提高篇】—— C++泛型编程之模板基本语法和使用的详解

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档文章目录前言一、模板的概念二、函数模板2.1 函数模板的使用2.2 函数模板注意事项2.3 普通函数与函数模板的区别2.4 普通函数与函数模板的调用规则2.5 模板的局限性三、类模…...

编程日记 2025/6/2 15:31:07

WPS计算机二级•高效操作技巧

听说这里是目录哦斜线表头展示项目名称🍋‍🟩横排转竖排🍐批量删除表格空白行🍈方法一方法二建辅助列找空值能量站😚 斜线表头展示项目名称🍋‍🟩 选中单元格，单击右键➡️“设…...

编程日记 2025/6/2 22:13:25

【Maui】视图界面与数据模型绑定

文章目录前言一、问题描述二、解决方案三、软件开发（源码）3.1 创建模型3.2 视图界面3.3 控制器逻辑层四、项目展示前言 .NET 多平台应用 UI (.NET MAUI) 是一个跨平台框架，用于使用 C# 和 XAML 创建本机移动和桌面应用。使用 .NET MAUI&…...

编程日记 2025/6/4 10:36:11

vue3-sfc-loader 加载远程.vue文件(sfc)案例

注意事项 style标签如果增加了lang比如：lang“scss”，需要提供scss-loader的处理器，这个暂时没研究，我的处理方式是将动态模版的css放在了全局打包暂时还没有测试，后面测试了会同步更新安装vue3-sfc-loader npm inst…...

编程日记 2025/6/2 21:34:22

Hadoop美食推荐系统爬虫1.8w+数据协同过滤余弦函数推荐美食 Springboot Vue Element-UI前后端分离

Hadoop美食推荐系统爬虫1.8w数据协同过滤余弦函数推荐美食 Springboot Vue Element-UI前后端分离【Hadoop项目】 1. data.csv上传到hadoop集群环境 2. data.csv数据清洗 3.MapReducer数据汇总处理, 将Reducer的结果数据保存到本地Mysql数据库中 4. SpringbootEchartsMySQL 显…...

编程日记 2025/6/2 6:54:30

使用Linux驱动程序的fasync(文件异步通知机制)向用户空间发送SIGIO信号的学习记录

前言本文学习使用Linux驱动程序的fasync(文件异步通知机制)向用户空间发送SIGIO信号。 fasync(文件异步通知机制)名字的来历 fasync 是 “file asynchronous” 的缩写，意思是文件异步通知。这里的文件是指文件结构体struct file *file ，关于文件结…...

编程日记 2025/6/2 16:42:11

面试经验分享-回忆版某小公司

说说你项目中数据仓库是怎么分层的，为什么要分层？ 首先是ODS层，连接数据源和数据仓库，数据会进行简单的ETL操作，数据来源通常是业务数据库，用户日志文件或者来自消息队列的数据等中间是核心的数据仓库层&a…...

编程日记 2025/6/2 20:02:23

【算法学习笔记】35：扩展欧几里得算法求解线性同余方程

线性同余方程问题线程同余方程问题是指 a x ≡ b ( m o d m ) ax \equiv b~(mod~m) ax≡b (mod m)，给定 a a a、 b b b和 m m m，找到一个整数 x x x使得该方程成立，即使得 a x m o d m b ax~mod~mb ax mod mb，随便返回任何一个…...

编程日记 2025/5/31 21:02:52

ent.SetDatabaseDefaults()

在 AutoCAD 的 .NET API 中，ent.SetDatabaseDefaults() 这句代码通常用于将一个实体（Entity）对象的属性设置为与其所在的数据库（Database）的默认设置相匹配。这意味着，该实体将采用数据库级别的默认颜色、图…...

编程日记 2025/6/2 22:13:57

使用docker部署tomcat服务器和mysql数据库

使用docker部署tomcat服务器 1、拉去tomcat镜像 [rootlocalhost yum.repos.d]# sudo docker pull docker.io/tomcat:9 9: Pulling from library/tomcat de44b265507a: Pull complete 4c2afd91a87d: Pull complete 89e9bbcfa697: Pull complete 11be3e613582: Pull complet…...

编程日记 2025/5/29 4:44:34

Jenkins 启动

废话这一阵子感觉空虚，心里空捞捞的，总想找点事情做，即使这是一件微小的事情，空余时间除了骑车、打球，偶尔朋友聚会 … 还能干什么呢？ 当独自一人时，究竟可以做点什么，填补这空虚…...

编程日记 2025/6/3 16:16:19

Elasticsearch（ES）基础查询语法的使用

1. Match Query (全文检索查询) 用于执行全文检索，适合搜索文本字段。 { “query”: { “match”: { “field”: “value” } } } match_phrase：精确匹配短语，适合用于短语搜索。 { “query”: { “match_phrase”: { “field”: “text” }…...

编程日记 2025/6/1 19:07:07

SpringCloud系列教程：微服务的未来（十四）网关登录校验、自定义过滤器GlobalFilter、GatawayFilter

前言在微服务架构中，API 网关扮演着至关重要的角色，负责路由请求、执行安全验证、流量控制等任务。Spring Cloud Gateway 作为一个强大的网关解决方案，提供了灵活的方式来实现这些功能。本篇博客将重点介绍如何在 Spring Cloud Gateway 中…...

编程日记 2025/6/2 6:49:36

Android Studio：Linux环境下安装与配置

更多内容：XiaoJ的知识星球 Android Studio：Linux环境下安装与配置 1.安装JDK2.安装Android Studio2.1 获取安装包2.2 安装（1）配置环境变量：（2）运行安装：（3）配…...

编程日记 2025/6/3 4:58:22

使用AI生成金融时间序列数据：解决股市场的数据稀缺问题并提升信噪比

“GENERATIVE MODELS FOR FINANCIAL TIME SERIES DATA: ENHANCING SIGNAL-TO-NOISE RATIO AND ADDRESSING DATA SCARCITY IN A-SHARE MARKET” 论文地址：https://arxiv.org/pdf/2501.00063 摘要金融领域面临的数据稀缺与低信噪比问题，限制了深度学习在…...

编程日记 2025/6/3 6:31:09

【银河麒麟高级服务器操作系统】业务访问慢网卡丢包现象分析及处理过程

了解更多银河麒麟操作系统全新产品，请点击访问麒麟软件产品专区：product.kylinos.cn 开发者专区：developer.kylinos.cn 文档中心：document.kylinos.cn 交流论坛：forum.kylinos.cn 服务器环境以及配置【内核版本…...

编程日记 2025/6/2 18:01:24

如何将数据库字符集改为中文，让今后所有的数据库都支持中文

最后一行有我自己的my.ini文件数据库输入中文数据时会变为乱码， 这个时候，我们为每个数据库设置字符集，太过于麻烦，为数据库单独设置重启后又会消失 Set character_set_database’utf8’; Set character_set_server’utf8’; …...

编程日记 2025/6/4 8:44:14

Linux-C/C++--深入探究文件 I/O (下)(文件共享、原子操作与竞争冒险、系统调用、截断文件)

经过上一章内容的学习，了解了 Linux 下空洞文件的概念；open 函数的 O_APPEND 和 O_TRUNC 标志；多次打开同一文件；复制文件描述符；等内容本章将会接着探究文件IO，讨论如下主题内容。  文件共享介绍&…...

编程日记 2025/5/28 9:02:57

Linux Bash 中使用重定向运算符的 5 种方法

注：机翻，未校。 Five ways to use redirect operators in Bash Posted: January 22, 2021 | by Damon Garn Redirect operators are a basic but essential part of working at the Bash command line. See how to safely redirect input and output t…...

编程日记 2025/6/2 6:49:00

opengrok_windows_环境搭建

目录软件列表软件安装工程索引编辑工程部署问题列表软件列表软件名下载地址用途JDKhttps://download.java.net/openjdk/jdk16/ri/openjdk-1636_windows-x64_bin.zipindex 使用java工具tomcathttps://dlcdn.apache.org/tomcat/tomcat-9/v9.0.98/bin/apache-tom…...

编程日记 2025/6/3 16:21:59

【无法下载github文件】虚拟机下ubuntu无法拉取github文件

修改hosts来进行解决。步骤一：打开hosts文件 sudo vim /etc/hosts步骤二：查询 github.com的ip地址 https://sites.ipaddress.com/github.com/#ipinfo将github.com的ip地址添加到hosts文件末尾，如下所示。 140.82.114.3 github.com步骤三…...

编程日记 2025/6/2 17:53:19

python——句柄

一、概念句柄指的是操作系统为了标识和访问对象而提供的一个标识符，在操作系统中，每个对象都有一个唯一的句柄，通过句柄可以访问对象的属性和方法。例如文件、进程、窗口等都有句柄。在编程中，可以通过句柄来操作这些对象&#x…...

编程日记 2025/6/2 19:14:01

.Net Core微服务入门系列（一）——项目搭建

系列文章目录 1、.Net Core微服务入门系列（一）——项目搭建 2、.Net Core微服务入门全纪录（二）——Consul-服务注册与发现（上） 3、.Net Core微服务入门全纪录（三）——Consul-服务注…...

编程日记 2025/6/3 16:23:19

Net Core微服务入门全纪录（三）——Consul-服务注册与发现（下）

编程日记 2025/6/3 16:23:51

[苍穹外卖] 1-项目介绍及环境搭建

项目介绍定位：专门为餐饮企业（餐厅、饭店）定制的一款软件产品功能架构： 管理端 - 外卖商家使用用户端 - 点餐用户使用技术栈： 开发环境的搭建整体结构： 前端环境前端工程基于 nginx 运行 - Ngi…...

编程日记 2025/6/3 16:22:16

【PCIe 总线及设备入门学习专栏 2 -- PCIe 的 LTSSM 和 Enumeration】

文章目录 OverviewLTSSM StatesDetect StatesDETECT_QUIETDETECT_ACTDETECT_WAITPolling StatesPOLL_ACTIVEPOLL_CONFIGPOLL_COMPLIANCEConfiguration StatesCONFIG_LINKWD_STARTCONFIG_LINKWD_ACCEPTCONFIG_LANENUM_WAITCONFIG_LANENUM_ACCEPTCONFIG_COMPLETECONFIG_IDLERecov…...

编程日记 2025/6/3 13:56:47

Redis 性能优化：多维度技术解析与实战策略

文章目录 1 基准性能2 使用 slowlog 优化耗时命令3 big key 优化4 使用 lazy free 特性5 缩短键值对的存储长度6 设置键值的过期时间7 禁用耗时长的查询命令8 使用 Pipeline 批量操作数据9 避免大量数据同时失效10 客户端使用优化11 限制 Redis 内存大小12 使用物理机而非虚拟机…...

编程日记 2025/6/3 16:23:35

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. 测试在恒定加速度运行下的预积分情况

相关文章：