IMU预积分【SLAM】

前言

预积分的推导过程比较多，所以这里只记录关键结论。
其实这些公式不太好记忆，因为预积分推导过程的想法来源很巧妙，无法看出物理意义。

预积分定义式（必须记住）

一切推导的来源：
最好记忆的旋转相对运动量：
$$△R_{ij}\doteq R_i^T{R}_j=\prod _{k=i}^{j-1}Exp(({\tilde{\omega }}_k-b_{g,k}-{\eta }_{gd,k})△t)$$
$$△v_{ij}\doteq R_i^T(v_j-v_i-g△t_{ij})=\sum _{k=i}^{j-1}△R_{ik}({\tilde{a}}_k-b_{a,k}-{\eta }_{ad,k})△t$$
$$△p_{ij}\doteq R_i^T\left(p_j-p_i-v_i△t_{ij}-\frac{1}{2}g△t_{ij}^2\right)=\sum _{k=i}^{j-1}\left[△v_{ik}△t+\frac{1}{2}△R_{ik}\left({\tilde{a}}_k-b_{a,k}-{\eta }_{ad,k}\right)△t^2\right]$$

${\eta }_{gd,k}$和${\eta }_{ad,k}$是离散化后的随机游走噪声（即建模为零均值白噪声高斯过程的测量噪声，因为也是角度或速度的随机游走所以才会这样描述）。

预积分的意义

IMU本身的数学模型中包括测量方程和噪声模型，再加上IMU运动学方程（本质上是微分方程组），可以推导出IMU在$△t$时间段内的直接积分公式，在滤波方法中可以用直接积分作为预测，但是在优化方法中，我们并不希望优化过程随着IMU数据进行调用，这样做太浪费计算资源，我们更希望将IMU数据测量值组合在一起处理，所以累积的IMU观测就是所谓的IMU预积分。

直接积分的缺点是，它描述的过程和状态量有关，如果对$i$时刻的状态进行优化，那么随后的所有IMU时刻的状态也会变化，整个积分要重新计算。

而通过预积分的定义可以看到，预积分定义最右侧公式与状态量无关（除了零偏，零偏发生变化时有单独的更新方法），所以当状态量发生变化时，也无需重新计算预积分量，更不必计算各个IMU时刻的状态量。此外预积分是累加或累乘的形式，给计算层面带来很大便利。

预积分的测量模型和噪声模型

推导过程假设了预积分过程中**零偏固定不变（所以下面公式中是$b_{g,i}$或$b_{a,i}$）**进行处理，实际零偏发生变化时预积分有单独的更新方式。

旋转部分

预积分旋转观测量（记住）：
$$△{\tilde{R}}_{ij}\doteq \prod _{k=i}^{j-1}Exp(({\tilde{\omega }}_k-b_{g,i})△t)$$

旋转噪声项：
$$Exp(-\delta {\varphi }_{ij})\doteq \prod _{k=i}^{j-1}Exp(-△{\tilde{R}}_{k+1,j}^T{J}_{r,k}{\eta }_{gd,k}△t)$$
$J_{r,k}$由BCH公式引入，和${\omega }_k$、$b_{g,k}$有关（假设条件使得$b_{g,k}=b_{g,i}$）。随机变量$\delta {\varphi }_{ij}$只和随机变量${\eta }_{gd}$有关，${\eta }_{gd}$是均值为$0$的白噪声，所以$\delta {\varphi }_{ij}$均值为$0$。

然后有：
$$△R_{ij}=△{\tilde{R}}_{ij}Exp(-\delta {\varphi }_{ij})$$

旋转噪声项递推更新公式：
$$\delta {\varphi }_{ij}=△{\tilde{R}}_{j-1}^T\delta {\varphi }_{i,j-1}+J_{r,j-1}{\eta }_{gd,j-1}△t$$

旋转噪声项的协方差递推公式：
设$j-1$时刻$\delta {\varphi }_{i,j-1}$的协方差为${\Sigma }_{j-1}$，高斯白噪声${\eta }_{gd}$的协方差为${\Sigma }_{{\eta }_{gd}}$（一般假设随机游走过程中高斯白噪声协方差保持不变，所以不区分${\eta }_{gd,k}$）。
$${\Sigma }_j=△{\tilde{R}}_{j-1,j}^T{\Sigma }_{j-1}△{\tilde{R}}_{j-1,j}+J_{r,j-1}{\Sigma }_{{\eta }_{gd}}{J}_{r,j-1}^T△t^2$$

速度部分

预积分速度观测量（记住）：
$$△{\tilde{v}}_{ij}\doteq \sum _{k=i}^{j-1}△{\tilde{R}}_{ik}({\tilde{a}}_k-b_{a,i})△t$$
速度噪声项：
$$-\delta v_{ij}\doteq \sum _{k=i}^{j-1}\left[△{\tilde{R}}_{ik}({\tilde{a}}_k-b_{a,i}{)}^{\wedge }\delta {\varphi }_{ik}△t-△{\tilde{R}}_{ik}{\eta }_{ad,k}△t\right]$$
然后有：
$$△v_{ij}=△{\tilde{v}}_{ij}-\delta v_{ij}$$
速度噪声项递推公式：
$$\delta v_{ij}=\delta v_{i,j-1}-△{\tilde{R}}_{i,j-1}({\tilde{a}}_{j-1}-b_{a,i}{)}^{\wedge }\delta {\varphi }_{i,j-1}△t+△{\tilde{R}}_{i,j-1}{\eta }_{ad,j-1}△t$$

位置（位移）部分

预积分位置（位移）观测量（记住）：
$$△{\tilde{p}}_{ij}\doteq \sum _{k=i}^{j-1}\left[(△{\tilde{v}}_{ik}△t)+\frac{1}{2}△{\tilde{R}}_{ik}({\tilde{a}}_k-b_{a,i})△t^2\right]$$
位置（位移）噪声项：
$$-\delta p_{ij}\doteq \sum _{k=i}^{j-1}\left[-\delta v_{ik}△t+\frac{1}{2}△{\tilde{R}}_{ik}({\tilde{a}}_k-b_{a,i}{)}^{\wedge }\delta {\varphi }_{ik}△t^2-\frac{1}{2}△{\tilde{R}}_{ik}{\eta }_{ad,k}△t^2\right]$$
然后有：
$$△p_{ij}=△{\tilde{p}}_{ij}-\delta p_{ij}$$
位置（位移）噪声项递推公式：
$$\delta p_{ij}=\delta p_{i,j-1}+\delta v_{i,j-1}△t-\frac{1}{2}△{\tilde{R}}_{i,j-1}({\tilde{a}}_{j-1}-b_{a,i}{)}^{\wedge }\delta {\varphi }_{i,j-1}△t^2+\frac{1}{2}△{\tilde{R}}_{i,j-1}{\eta }_{ad,j-1}△t^2$$

重要说明（重要！！！）

1、首先需要记住的公式均是预积分的定义式和观测量的定义式，其实二者形式几乎是相同的，只是观测量把定义式中的随机变量部分分离出去形成了噪声项。
2、说明一下本人对于预积分应用的理解，类比其他SLAM问题中的后端优化问题模型。可以看到预积分的公式可以写成“观测量=状态量推导的预测量+噪声”的形式，比如$△{\tilde{R}}_{ij}=△Exp(-\delta {\varphi }_{ij})$，这个公式左侧的观测量由传感器观测值利用观测量定义式计算，右侧的预测量由本文第一部分中预积分定义式的中间公式计算，这样观察会发现，IMU预积分的数学模型和其他SLAM后端优化模型完全一样，求观测值-预测值的最小二乘问题其实就是求预积分的最大后验估计，而噪声项的协方差就是通过高斯分布的概率密度函数引入到优化问题中，指导着残差在求解优化问题中的比重。如果不采用完全准确的协方差，该模型将不会严格匹配预积分的最大后验估计，但是仍然具有最小二乘的意义，只不过残差在求解优化问题中的权重是自定义决定的。

预积分对于零偏的更新

前面的推导过程中，在观测量的定义部分假设了零偏不变，因此考虑零偏变化时观测量和零偏的关系。思路就是假设预积分观测和零偏之间是线性关系，利用泰勒展开保留一阶项（旋转部分的更新方式稍有不同）。重点在于雅可比矩阵的计算，最后还要把雅可比矩阵写成递推关系便于计算。

实际应用中，当优化后零偏发生变化时，利用递推公式先求雅可比矩阵，然后更新到预积分观测量上去。
公式太多了就不敲了（应该也不用记住）。

预积分残差对状态量的雅可比矩阵

这个公式也不敲了，需要的时候查书就可以了。

总结

在实际应用中，如果要进行IMU预积分，当一帧IMU信息到来时需要做：
1、计算三个预积分观测量
2、计算三个噪声项的协方差矩阵，作为后续图优化的信息矩阵
3、计算预积分观测量对零偏的雅可比矩阵，一共五个。
结束预积分计算后，可以把这些结果取出并在优化过程中使用。