IMU预积分关键部分介绍

简单记录《自动驾驶与机器人中的SLAM技术》一书中IMU预积分算法，防止自己遗忘。

概述

当一个新的IMU数据到来时，预积分器应该完成以下功能：

1. 在上一个数据的基础上利用预积分测量模型计算三个预积分观测量$\Delta {\tilde{R}}_{ij},\Delta {\tilde{v}}_{ij},\Delta {\tilde{p}}_{ij}$。
2. 计算三个噪声量的协方差矩阵 ${\Sigma }_{i,k+1}$，作为后续图优化的信息矩阵。
3. 计算预积分观测量相对于零偏的雅可比矩阵，共五个，用于在零偏更新后更新预积分观测量。

预积分测量模型

$$\begin{gathered} \Delta {\tilde{R}}_{ij}=\prod _{k=i}^{j-1}\text{Exp}\left(({\tilde{\omega }}_k-b_{g,i})\Delta t\right) \\ \Delta {\tilde{v}}_{ij}=\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}({\tilde{a}}_k-b_{a,i})\Delta t \\ \Delta {\tilde{p}}_{ij}=\sum _{k=i}^{j-1}\left[(\Delta {\tilde{v}}_{ik}\Delta t)+\frac{1}{2}\Delta {\tilde{R}}_{ik}({\tilde{a}}_k-b_{a,i})\Delta t^2\right] \end{gathered}$$
使用上述三个公式输入IMU观测值计算预积分量(不考虑噪声，也就是说计算出来的是观测量，包含着观测噪声)
上式中预积分内部带有零偏量，因此不可避免地会依赖此时的零偏估计，为了处理这种依赖做出如下调整：
1、$i$时刻零偏是固定的，并且在整个预积分计算过程中都是固定的
2、做出预积分对零偏量的一阶线性化模型，舍弃对零偏的高阶项
3、当零偏估计发生变化时，用这个线性模型来修正预积分

预积分噪声模型

这一部分主要是为了构建噪声递推公式，得到AB系数矩阵在每次递推时用来更新噪声协方差矩阵，该噪声协方差矩阵用于预积分图优化中的信息矩阵（GN优化方法中$H=J^{T}PJ=J^T{\Sigma }^{-1}J,$ 误差越大权重越小）。
噪声模型：
$$\begin{gathered} \delta {\varphi }_{i,j}=\Delta {\tilde{R}}_{r,j-1,j}^{\top }\delta {\varphi }_{i,j-1}+J_{r,j-1}{\eta }_{gd,j-1}\Delta t \\ \delta v_{i,j}=\delta v_{i,j-1}-\Delta {\tilde{R}}_{i,j-1}({\tilde{a}}_{j-1}-b_{a,i}{)}^{\wedge }\delta {\varphi }_{i,j-1}\Delta t+\Delta {\tilde{R}}_{vi,j-1}{\eta }_{ad,j-1}\Delta t \\ \delta p_{i,j}=\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 {\varphi }_{i,j-1}\Delta t^2+\frac{1}{2}\Delta {\tilde{R}}_{i,j-1}{\eta }_{ad,j-1}\Delta t^2 \end{gathered}$$
转换为递推公式
$$\left[\begin{array}{c}\delta {\varphi }_{i,j}\\ \delta v_{i,j}\\ \delta p_{i,j}\end{array}\right]=\left[\begin{array}{ccc}\Delta {\tilde{R}}_{i,j-1}^{-1}& 0& 0\\ -\Delta {\tilde{R}}_{i,j-1}({\tilde{a}}_{j-1}-b_{a,i}{)}^{\delta {\varphi }_{ik}}& I& 0\\ -\frac{1}{2}\Delta {\tilde{R}}_{i,j-1}({\tilde{a}}_{j-1}-b_{a,i}{)}^{\delta {\varphi }_{ik}}\Delta t& \Delta t& I\end{array}\right]\left[\begin{array}{c}\delta {\varphi }_{i,j-1}\\ \delta v_{i,j-1}\\ \delta p_{i,j-1}\end{array}\right]+\left[\begin{array}{cc}{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}\end{array}\right]\left[\begin{array}{c}{\eta }_{gd,j-1}\\ {\eta }_{ad,j-1}\end{array}\right]$$
噪声的协方差递推公式为
$${\Sigma }_{i,k+1}=A_{k+1}{\Sigma }_{i,k}{A}_{k+1}^{\top }+B_{k+1}Cov({\eta }_{d,k})B_{k+1}^{\top }$$
这里的 $A_{k+1}$ 矩阵接近单位阵 $I$，因此可以看成噪声累加。陀螺仪的噪声通过 $B$ 矩阵进入旋转的观测量中，加速度计的噪声则主要进入速度与平移估计中。

零偏的更新

前面的预积分观测模型中假设了在 $i$ 时刻的IMU零偏恒定不变，这里我们假定预积分观测是随零偏线性变化的，也就是说在零偏发生时，按照一阶导数来更新预积分观测量。
具体来说就是把预积分观测量看成零偏 $b_{g,i}$, $b_{a,i}$, 的函数，那么当零偏更新时，预积分观测量做如下修改：
$$\begin{gathered} \Delta {\tilde{R}}_{ij}(b_{g,i}+\delta b_{g,i})=\Delta {\tilde{R}}_{ij}(b_{g,i})\text{Exp}\left(\frac{\partial \Delta {\tilde{R}}_{ij}}{\partial b_{g,i}}\delta b_{g,i}\right) \\ \Delta {\tilde{v}}_{ij}(b_{g,i}+\delta b_{g,i},b_{a,i}+\delta b_{a,i})=\Delta {\tilde{v}}_{ij}(b_{g,i},b_{a,i})+\frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{g,i}}\delta b_{g,i}+\frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{a,i}}\delta b_{a,i} \\ \Delta {\tilde{p}}_{ij}(b_{g,i}+\delta b_{g,i},b_{a,i}+\delta b_{a,i})=\Delta {\tilde{p}}_{ij}(b_{g,i},b_{a,i})+\frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{g,i}}\delta b_{g,i}+\frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{a,i}}\delta b_{a,i} \end{gathered}$$
其中上述公式中偏导数的递推形式如下：
$$\begin{gathered} \frac{\partial \Delta {\tilde{R}}_{ij}}{\partial b_{g,i}}=\Delta {\tilde{R}}_{j-1,j}^{\top }\frac{\partial \Delta {\tilde{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}({\tilde{a}}_{j-1}-b_{a,i}{)}^{\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}({\tilde{a}}_{j-1}-b_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{R}}_{i,j-1}}{\partial b_{g,i}}\Delta t^2 \end{gathered}$$
每次接收到新的IMU数据时都计算观测量对$i$时刻零偏的偏导数，以便在零偏更新后（预积分优化）更新预积分观测值。

基于预积分的图优化

这里是将IMU的预积分作为约束因子（边）建立因子图进行图优化，要建立约束需要知道节点、残差、雅可比；

1、节点是状态变量PVQ及两个零偏

$$x={\left[\begin{array}{cccccccc}{R}_i& p_i& v_i& b_{gi}& b_{ai}& R_j& p_j& v_j\end{array}\right]}^{\top }$$

2、残差是状态变量的误差方程

噪声模型中我们给出了噪声模型的递推公式，是为了更新噪声的协方差矩阵，下边我们先给出测量模型，然后由测量模型给出误差方程：
$$\begin{gathered} \Delta {\tilde{R}}_{ij}=R_i^{\top }{R}_{j}Exp(\delta {\varphi }_{ij}), \\ \Delta {\tilde{v}}_{ij}=R_i^{\top }(v_j-v_i-g\Delta t_{ij})+\delta v_{ij}, \\ \Delta {\tilde{p}}_{ij}=R_i^{\top }\left(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}g\Delta t_{ij}^2\right)+\delta p_{ij}. \end{gathered}$$
以上公式可以看出测量值等于真实值加噪声，该公式是由真实值公式推导出来的（分离测量与噪声），下面从测量模型定义残差：
$$\begin{gathered} r_{\Delta R_{ij}}=Log\left((\Delta {\tilde{R}}_{ij})(R_i^{\top }{R}_j)\right)=-\delta {\varphi }_{ij}, \\ {r}_{\Delta v_{ij}}=R_i^{\top }(v_j-v_i-g\Delta t_{ij})-\Delta {\tilde{v}}_{ij}=-\delta v_{ij}, \\ {r}_{\Delta p_{ij}}=R_i^{\top }\left(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}g\Delta t_{ij}^2\right)-\Delta {\tilde{p}}_{ij}=-\delta p_{ij}. \end{gathered}$$
上式中，残差部分实际就是噪声部分，因此残差的协方差矩阵就是噪声的协方差矩阵。

3、雅可比是残差对状态变量的偏导

$${\left[\begin{array}{c}\frac{\partial r_{\Delta R_{ij}}}{\partial x}\\ \frac{\partial r_{\Delta v_{ij}}}{\partial x}\\ \frac{\partial r_{\Delta p_{ij}}}{\partial x}\end{array}\right]}_{9\ast 24}=\left[\begin{array}{cccccccc}\frac{\partial r_{\Delta R_{ij}}}{\partial R_i}& \frac{\partial r_{\Delta R_{ij}}}{\partial p_i}=0& \frac{\partial r_{\Delta R_{ij}}}{\partial v_i}=0& \frac{\partial r_{\Delta R_{ij}}}{\partial b_{gi}}& \frac{\partial r_{\Delta R_{ij}}}{\partial b_{ai}}=0& \frac{\partial r_{\Delta R_{ij}}}{\partial R_j}& \frac{\partial r_{\Delta R_{ij}}}{\partial p_j}=0& \frac{\partial r_{\Delta R_{ij}}}{\partial v_j}=0\\ & & & & & & & \\ \frac{\partial r_{\Delta v_{ij}}}{\partial R_i}& \frac{\partial r_{\Delta v_{ij}}}{\partial p_i}=0& \frac{\partial r_{\Delta v_{ij}}}{\partial v_i}& \frac{\partial r_{\Delta v_{ij}}}{\partial b_{gi}}& \frac{\partial r_{\Delta v_{ij}}}{\partial b_{ai}}& \frac{\partial r_{\Delta v_{ij}}}{\partial R_j}=0& \frac{\partial r_{\Delta v_{ij}}}{\partial p_j}=0& \frac{\partial r_{\Delta v_{ij}}}{\partial v_j}\\ & & & & & & & \\ \frac{\partial r_{\Delta p_{ij}}}{\partial R_i}& \frac{\partial r_{\Delta p_{ij}}}{\partial p_i}& \frac{\partial r_{\Delta p_{ij}}}{\partial v_i}& \frac{\partial r_{\Delta p_{ij}}}{\partial b_{gi}}& \frac{\partial r_{\Delta p_{ij}}}{\partial b_{ai}}& \frac{\partial r_{\Delta p_{ij}}}{\partial R_j}=0& \frac{\partial r_{\Delta p_{ij}}}{\partial p_j}& \frac{\partial r_{\Delta p_{ij}}}{\partial v_j}=0\end{array}\right]$$
其中：
$$\begin{gathered} 残差对R_i求偏导: \\ \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 v_{ij}}}{\partial R_i}=(R_i^{\top }(v_j-v_i-g\Delta t_{ij}){)}^{\wedge } \\ \frac{\partial r_{\Delta p_{ij}}}{\partial R_i}=(R_i^{\top }(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}g\Delta t_{ij}^2){)}^{\wedge } \end{gathered}$$
$$\begin{gathered} 残差对p_i求偏导: \\ \frac{\partial r_{\Delta p_{ij}}}{\partial p_i}=-R_i^{\top } \end{gathered}$$
$$\begin{gathered} 残差对v_i求偏导: \\ \frac{\partial r_{\Delta v_{ij}}}{\partial v_i}=-R_i^{\top } \\ \frac{\partial r_{\Delta p_{ij}}}{\partial v_i}=-R_i^{\top }\Delta t_{ij} \end{gathered}$$
$$\begin{gathered} 残差对b_{g,i}求偏导: \\ \frac{\partial r_{\Delta R_{ij}}}{\partial b_{g,i}}=-J_r^{-1}(r_{\Delta R_{ij}}^{\prime })Exp{\left(r_{\Delta R_{ij}}^{\prime }\right)}^{\top }\frac{\partial {\hat{R}}_{ij}}{\partial b_{g,i}} \\ \frac{\partial r_{\Delta v_{ij}}}{\partial b_{g,i}}=-\left(\frac{\partial \Delta {\tilde{v}}_{i,j-1}}{\partial b_{g,i}}-\Delta {\tilde{R}}_{i,j-1}({\tilde{a}}_{j-1}-b_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{R}}_{i,j-1}}{\partial b_{g,i}}\Delta t\right) \\ \frac{\partial r_{\Delta p_{ij}}}{\partial b_{g,i}}=-\left(\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}({\tilde{a}}_{j-1}-b_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{R}}_{i,j-1}}{\partial b_{g,i}}\Delta t^2\right) \end{gathered}$$
$$\begin{gathered} 残差对b_{a,i}求偏导: \\ \frac{\partial r_{\Delta v_{ij}}}{\partial b_{a,i}}=-\left(\frac{\partial \Delta {\tilde{v}}_{i,j-1}}{\partial b_{a,i}}-\Delta {\tilde{R}}_{i,j-1}\Delta t\right) \\ \frac{\partial r_{\Delta p_{ij}}}{\partial b_{a,i}}=-\left(\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\right) \end{gathered}$$
$$\begin{gathered} 残差对R_j求偏导: \\ \frac{\partial r_{\Delta R_{ij}}}{\partial R_j}=J_r^{-1}(r_{\Delta R_{ij}}) \end{gathered}$$
$$\begin{gathered} 残差对p_j求偏导: \\ \frac{\partial r_{\Delta p_{ij}}}{\partial p_j}=R_i^{\top } \end{gathered}$$
$$\begin{gathered} 残差对v_j求偏导: \\ \frac{\partial r_{\Delta p_{ij}}}{\partial p_j}=R_i^{\top } \end{gathered}$$