预积分笔记
早前学习的高博的简明预积分，推导了不明白的地方，结合loam的代码的学习笔记。
本章要介绍一种在紧耦合系统中十分常见的IMU数据处理方法：预积分（Pre-integraion）[1]。与传统IMU的运动学积分不同，预积分可以将一段时间内的IMU测量数据累计起来，建立预积分测量，因而十分适合以关键帧为基础单元的激光或视觉SLAM系统。如今预积分已经成为诸多与IMU紧耦合的标准方法[2,3,4]，下面我们来介绍其基本原理。
一、预积分的定义
在一个IMU系统里，我们考虑它的五个变量：旋转 、平移 、角速度 、线速度 与加速度 。根据旋转运动学，这些变量的运动学关系可以写成[5]：
第一个式子的证明：
向量得到反对称矩阵，性质：叉乘转换为点乘，旋转矩阵与向量相乘后的反对称矩阵
Pb是local frame（B）下的点和imu一起动在B下的位置不变，求导为0.PA为相同点在global frame（A）下的表示会变。
线速度和角速度的关系：
7带入6：
将9式带入8得到证明。
PB是任意点因此可以去掉。
在 t到t+\triangledown{t} 时间内，对上式进行欧拉积分，可得：
上面的第一项wt右乘是将R在其原有的坐标系上旋转（右乘相对于右边位姿的自身坐标系）
其中角速度和加速度可以被IMU测量到，但受到噪声与重力影响。令测量值为{\omega}^~和 a^~，则：
imu测量的是非重力加速度，a是总体的加速度，因此需要减去g，然后转换到imu所在的坐标系上来乘以RT。
其中b_{g}\ \ b_{a} 为陀螺和加速度计零偏，{\eta}_{g}\ {\eta}_{a} 为测量的高斯噪声。把该式代入上式，可得到测量值与状态变量的关系：
其中 是离散化后的随机游走噪声[6]:
以上过程与我们在IMU测量方程和噪声方程中已有描述。当然，我们完全可以用这种约束来构建图优化，对IMU相关的问题进行求解。但是这组方程刻画的时间太短，或者说，IMU的测量频率太高。我们并不希望优化过程随着IMU数据进行调用(来一次数据优化一次)，那样太浪费计算资源。于是，预积分方法应运而生，它可以把一段时间的IMU数据累计起来统一处理。现在介绍如何在关键帧之间对IMU进行预积分。
我们不妨假设从关键帧 i 和 j 之间的IMU数据被累计起来（i j之间采用普通积分，不做优化），这个过程通常可以持续若干秒钟。这种被累计起来的观测被称为预积分[7]。当然，如果我们使用不同形式的运动学，得到的预积分形式也是不同的。本文主要使用 SO(3)+t 的方式来推导预积分。那么，在 k 至j 过程中，我们可以把上式中的变量累计起来，得到：
得到下面的过程是在i，j之间插入了测量数据，没加入测量数据利用上面的等式计算当前时刻k，下一个时刻k+1的R V P得到：
其中{\sum_{k=i}^{j-1} \triangle{t}}=t_{i\ j} ，为累计起来的时间。在已知 i 时刻状态和所有测量时，该式可以用于推断j 时刻的状态。当然，这只是运动学式的累计形式，并无本质不同。这就是传统意义上的直接积分。
直接积分的缺点是，如果我们对i 时刻的状态进行优化，那么 i,i+1,i+2,,,j 时刻的状态也会跟着发生改变，这个积分就必须重新计算[8]，这是非常不便的。为此，我们对上式稍加改变，定义相对的运动量为：
最后一个等式证明：
这种改变实际上只是计算了某种从 i 到 j 的“差值”。这个定义在计算上有一些有趣的性质：
- 我们不妨考虑从i 时刻出发，此时这三个量都为零。在 i+1时刻，我们计算出 {\triangledown}R_{i\ i+1},{\triangledown}v_{i,i+1}和 {\triangledown}p_{i,i+1}。而在 i+2时刻时，由于这三个式子都是累乘或累加的形式，只需在 i+1时刻的结果之上，加上第i+2时刻的测量值即可。这在计算层面带来了很大的便利。进一步，我们还会发现这种性质对后续计算各种雅可比矩阵都非常方便。
- 上述所有计算都和{\triangledown}R_{i} 的取值无关。即使{\triangledown}R_{i} 的估计值发生改变，这些量也无需重新计算。这又是非常方便的一个特性。
- 不过，如果零偏 b_{g,k}\ \ b_{a,k} 发生变化(因为上面等式中每个步骤都含有零偏)，那么上述式子理论上还是需要重新计算。然而，我们也可以通过“修正”而非“重新计算”的思路，来调整我们的预积分量。
- 请注意，预积分量并没有直接的物理含义（上面的\Delta没有物理意义，是凑出来的）。尽管符号上用了 之类的样子，但它并不表示某两个速度或位置上的偏差。它只是如此定义而已。当然，从量纲上来说，应该与角度、速度、位移对应。
- 同样地，由于预积分量不是直接的物理量，这种“测量模型”的噪声也必须从原始的IMU噪声推导而来。
从这几个问题出发，我们来介绍如何构造预积分的测量模型、噪声模型，以及它对各种状态变量的雅可比应该如何方便地计算。
二、预积分测量模型
预备知识
构建预积分测量模型
由前面的讨论可见，预积分内部带有IMU的零偏量，因此不可避免地会依赖此时的零偏量估计。为了处理这种依赖，我们对预积分定义作一些工程上的调整：
- 我们首先认为 i时刻的零偏是固定的，并且在整个预积分计算过程中也都是固定的。
对应预积分函数：MidPointIntegration中的代码：
//预积分阶段零偏假设为没变
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
2、我们作出预积分对零偏量的一阶线性化模型，即，舍弃对零偏量的高阶项。
3、当零偏估计发生改变时，用这个线性模型来修正预积分。
首先，我们固定 i 时刻的零偏估计，来分析预积分的噪声。无论是图优化还是滤波器技术，都需要知道某个测量量究竟含有多大的噪声。我们不妨从旋转开始计算，因为旋转相对来说比较容易。利用BCH展开，可以作出下列的近似：
对应预积分函数：MidPointIntegration中的代码：
//公式中使用连加是因为i j之间有间隔
//公式中使用的是测量值这里的gyr0 gyr1也是
Vector3d un_gyr = 0.5 * (gyr0 + gyr1) - linearized_bg;
//欧拉角转换为四元数，使用了近似算法，当a很小时 sina=a cosa=1
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * dt / 2, un_gyr(1) * dt / 2, un_gyr(2) * dt / 2);
我们舍掉上式中的噪声二阶小量，并且定义预积分速度观测量为：
对应预积分函数：MidPointIntegration中的代码：
//公式中使用连加是因为i j之间有间隔
//acc1 测量值
Vector3d un_acc_1 = result_delta_q * (acc1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
//加上上次的值，相当于连加了
result_delta_v = delta_v + un_acc * dt;
那么上式可以化简为：
最后，可以对平移部分定义类似的操作。按照平移部分的定义方式，将旋转和速度观测量代入，可得：
在第三式至第四式中我们舍去了二阶噪声小量。同前，我们定义预积分位移观测量为：
对应预积分函数：MidPointIntegration中的代码：
Vector3d un_acc_1 = result_delta_q * (acc1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
// un_acc 已经包含了公式中的R和（）就等于平方前面的式子
result_delta_p = delta_p + delta_v * dt + 0.5 * un_acc * dt * dt;
那么前面式子可以写成：
其中第二点：
Vector3d un_acc_0 = delta_q * (acc0 - linearized_ba);
Vector3d un_gyr = 0.5 * (gyr0 + gyr1) - linearized_bg;
//使用了近似算法，当a很小时 sina=a cosa=1，
/*
如果知道了 k 时刻的预积分观测量，又很容易根据k+1 时刻传感器读数，计算出k+1 时刻的预积分观测量。这是由观测的累加定义方式决定的。
*/
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * dt / 2, un_gyr(1) * dt / 2, un_gyr(2) * dt / 2);
Vector3d un_acc_1 = result_delta_q * (acc1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
/*
如果知道了 k 时刻的预积分观测量，又很容易根据k+1 时刻传感器读数，计算出k+1 时刻的预积分观测量。这是由观测的累加定义方式决定的。
*/
result_delta_v = delta_v + un_acc * dt;
/*
如果知道了 k 时刻的预积分观测量，又很容易根据k+1 时刻传感器读数，计算出k+1 时刻的预积分观测量。这是由观测的累加定义方式决定的。
*/
result_delta_p = delta_p + delta_v * dt + 0.5 * un_acc * dt * dt;
//预积分阶段零偏假设为没变
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
下面我们就来解决这个问题。
三、预积分噪声模型
由于噪声项的定义比较复杂，本节会使用同样的思路来处理各种噪声项。我们会将复杂的噪声项线性化，保留一阶项系数，然后推导线性模型下的协方差矩阵变化。这是一种非常常见的处理思路，对许多复杂模型都很有效。
我们来回顾几个噪声项的定义方式。从旋转的噪声开始看：
上面式子的证明：
答案显然是肯定的。由于上式是累加形式的，很容易将其写成递推的形式：
由上一次的推导目前的
接下来我们考虑速度部分。与旋转部分相同，速度部分也可以写成高斯噪声变量的线性组合形式：
对于平移部分也可以做同样的处理。我们直接列写平移部分噪声的累加形式：
四、零偏的更新
这里的偏导数就是代码中的jacobian_，数据每到来一次可以更新jacobian_
对应函数：
Matrix<double, 15, 1> Evaluate(const Vector3d &Pi,
const Quaterniond &Qi,
const Vector3d &Vi,
const Vector3d &Bai,
const Vector3d &Bgi,
const Vector3d &Pj,
const Quaterniond &Qj,
const Vector3d &Vj,
const Vector3d &Baj,
const Vector3d &Bgj)
中的代码：
//trans p对零偏BA（加速度）导数
Matrix3d dp_dba = jacobian_.block<3, 3>(O_P, O_BA);
////trans p对零偏BG（陀螺仪数据）导数
Matrix3d dp_dbg = jacobian_.block<3, 3>(O_P, O_BG);
//旋转R对零偏BG（陀螺仪数据）导数
Matrix3d dq_dbg = jacobian_.block<3, 3>(O_R, O_BG);
//速度v对零偏BA（加速度）导数
Matrix3d dv_dba = jacobian_.block<3, 3>(O_V, O_BA);
//速度v对零偏BG（陀螺仪数据）导数
Matrix3d dv_dbg = jacobian_.block<3, 3>(O_V, O_BG);
//3*1 零偏的变化
Vector3d dba = Bai - linearized_ba_;
Vector3d dbg = Bgi - linearized_bg_; // NOTE: optimized one minus the linearized one
//零偏更新后要更新状态变量
Quaterniond corrected_delta_q = delta_q_ * DeltaQ(dq_dbg * dbg);
Vector3d corrected_delta_v = delta_v_ + dv_dba * dba + dv_dbg * dbg;
Vector3d corrected_delta_p = delta_p_ + dp_dba * dba + dp_dbg * dbg;
于是，问题就自然而然地变为，如何计算上面列写的几个偏导数（雅可比）呢？实际上非常简单，只需应用前面介绍的定义即可。下面我们来推导这几个雅可比矩阵。这个过程和我们先前把噪声变量移出来求线性化非常相似，读者不应感到陌生。首先来考虑旋转。预积分旋转观测量可以写为：
上面最后一步的证明：
接下来考虑速度测量：
可见速度相对于零偏的导数可以部分地由旋转导数的结果计算出来。
最后是平移部分：
这样我们就算出了平移对两个零偏变量的雅可比矩阵。请注意上述计算都是迭代形式的。也就是说，我们只需要在程序中保留一个变量，随着数据不断进行累加即可。
最后，我们把这些雅可比矩阵整理一下，得到更整齐的结果：
五、预积分模型归结至图优化
现在我们定义了预积分的测量模型，推导了它的噪声模型和协方差矩阵，并说明了随着零偏量更新，预积分应该怎么更新。事实上，我们已经可以把预积分观测作为图优化的因子（Factor）或者边（Edge）了。下面我们来说明如何使用这种边，以及它推导它相对于状态变量的雅可比矩阵。
残差定义如下，其中第一部分是PVQ增量的估计值，需要通过非IMU的方式获得，例如点云到Map的匹配，第二部分是PVQ增量的测量值，即通过第六章推导的“偏差更新时的预积分测量值更新”方法获得的修正后的测量值，这种近似的修正方式免去了积分的重新计算，是预积分降低计算量的关键。
公式中不含有零偏但是影响预积分的状态变量\Delta{R,v,p},因此更新零偏后需要更新状态变量\Delta{R,v,p},
再计算残差
请注意，在前面我们讨论了预积分测量关于IMU零偏的线性形式，因此在预积分残差定义式中，也完合可以将前面列写的线性近似式代入，从而得到预积分因子相比于IMU零偏的雅可比。由于那种写法会让整个式子变得复杂，所以这里我们没有特别地展开它。
六、预积分的雅可比矩阵
最后我们来讨论预积分相比于状态变量的雅可比矩阵。由于预积分测量已经归纳了IMU在短时间内的读数，残差相对于状态变量的雅可比推导显得十分简单，下面我们来推导它们。
利用上面的式子可以得到残差相对于\theta_{i} \ \ \ \theta_{j}的雅可比矩阵：
这些推导和位姿图的非常相似。而对于零偏量要稍微麻烦一些。注意在优化过程中，零偏量应该不断地更新，而每次更新时我们会利用当前零偏来修正预积分的观测量。由于这个过程是不断进行的，我们总会有一个初始的观测量和当前修正后的观测量，在推导时必须考虑到这一点。
所以我们最后得到：
参考文献：
[1]. C.Forster, L.Carlone, F.Dellaert,and D.Scaramuzza “IMU preintegration on manifold for efficient visual inertial maximum a posteriori estimation,” in Robotics: Science and Systems XI, no. EPFLCONF214687, 2015.
[2]. L. Chang, X. Niu, and T. Liu, “GNSS/imu/odo/lidarslam integrated navigation system using imu/odo pre integration,” Sensors, vol. 20, no. 17, p. 4702, 2020.
[3]. Z. Yuan, D. Zhu, C. Chi, J. Tang, C. Liao, and X. Yang, “Visual inertial state estimation with preintegration correction for robust mobile augmented reality,” in Proceedings of the 27th ACM International Conference on Multimedia, pp. 1410–1418, 2019.
[4]. K. Eckenhoff, P. Geneva, and G. Huang, “Closedform preintegration methods for graphbased visual-inertial navigation,” The International Journal of Robotics Research, vol. 38, no. 5, pp. 563–586, 2019.
[5]. R. M. Murray, Z. Li, and S. S. Sastry, A mathematical introduction to robotic manipulation. CRC press, 2017.
[6]. J. L. Crassidis, “Sigmapoint Kalman filtering for integrated GPS and inertial navigation,” IEEE Transactions on Aerospace and Electronic Systems, vol. 42, no. 2, pp. 750–756, 2006.
[7]. T.Luptonand, S.Sukkarieh, “Visual inertial aided navigation for highdynamic motion in built environments without initial conditions,” IEEE Transactions on Robotics, vol. 28, no. 1, pp. 61–76, 2011.
[8]. S.Leutenegger, S.Lynen, M.Bosse, R.Siegwart, and P.Furgale, “Keyframe based visual-inertial odometry using nonlinear optimization,” The International Journal of Robotics Research, vol. 34, no. 3, pp. 314–334, 2015.
本文使用 Zhihu On VSCode 创作并发布