IMU预积分理论推导

IMU与视觉或激光耦合是目前SLAM的常用形式。在后端优化中，我们需要通过IMU测量和状态量构建的残差，同时计算残差相对于状态量的雅可比矩阵，以及噪声量的协方差矩阵。为了减少计算量，IMU预积分是目前主要使用的一种形式。

基本思路

预积分的主要目的是使得IMU测量量与绝对位姿解耦，避免在位姿更新时需要更新测量量的积分。

IMU的输出为加速度和角速度，测量模型为（这里考虑以惯性坐标系为参考系）：

其中

\[n_a \sim N(0,\sigma_a^2),\ n_\omega \sim N(0,\sigma_\omega^2)\]

\[\dot{b}_{a_t}=n_{b_a},\ \dot{b}_{\omega_t}=n_{b_\omega}\]

\[n_{b_a} \sim N(0,\sigma_{b_a}^2),\ n_{b_\omega} \sim N(0,\sigma_{b_\omega}^2)\]

\(\sigma_a, \sigma_\omega, \sigma_{b_a}, \sigma_{b_\omega}\) 是IMU的参数，预先经过allen方差等方法标定后作为参数输入。

那么我们要建立测量值与状态量之间的关系显然需要积分，即通过IMU的运动模型：

上述公式是对连续值的积分，但事实上IMU测量值是一定频率的，离散的。假设\(t_i \ in [t_k,t_{k+1}]\) 是两个相机帧之间的IMU帧，积分公式可以转化为累加的形式：

（来源 https://zhuanlan.zhihu.com/p/388859808）

这里符号表示和上方不统一，但意思是一致的，即把原来的连续值积分转化为数值累加/累乘。

根据这个公式，我们事实上已经建立起状态量和测量值之间的关系，可以构建残差了。但问题时，一旦状态量更新，如\(R_i\)改变，那么由于速度、位置积分中有\(R_k\)，那i~j之间的速度，位置量必须重新积分，这增加了很大计算量。

预积分正是为了改进这个问题，它定义了一种相对的运动量（在vinsMono中，这种方式被表述为以k时刻的体坐标系为参考坐标系）。

预积分量并没有直接的物理含义。\(\Delta v, \Delta p\)并不表示某两个速度或位置上的偏差，只是如此定义而已。当然，从量纲上来说，应该与角度、速度、位移对应。

可以看到此时等式右侧现在与状态量中的位置，速度，姿态无关。此时更新状态量中的p, q, v，相邻两帧之间的IMU测量值约束也不需要重新计算。不过，如果零偏发生变化，那么还是需要重新积分的，但是考虑到零偏的变化程度比较小，我们能利用修正而非重新计算的思路来调整积分量。

预积分测量模型

在测量模型中，我们需要建立预积分测量和状态量之间的关系，同时分析其噪声。

旋转

我们定义预积分测量量

而我们定义的预积分状态量\(\Delta R_{ij}\)可以表示为：

不断地把观测置换到左侧，并把噪声置换到右侧，并且把噪声项内部\(\Delta \widetilde{R}\)的项合并，可以得到：

速度

定义预积分速度观测量为：

那么可以推导状态量与观测量之间的关系：

位置

定义预积分位置观测量为：

那么位置状态量与观测量的关系可以写为：

将它们代回最初的定义式，可以简单写为：

可以看到，这里左侧是定义的预积分测量量，只包含原始测量值和偏差，且是累加累乘的形式，很容易由k时刻推导到k+1时刻。从右侧看来，也很容易根据i和j时刻的状态变量来推测预测分观测量的大小，从而写出误差公式，形成最小二乘。

现在的问题是：预积分的噪声是否符合零均值的高斯分布？如果是，它的协方差有多大？和IMU本身的噪声之间是什么关系

预积分噪声模型

参考 https://zhuanlan.zhihu.com/p/388859808

噪声的协方差矩阵

零偏的更新

参考 https://zhuanlan.zhihu.com/p/388859808

预积分的雅可比矩阵

参考 https://zhuanlan.zhihu.com/p/388859808