Vins-mono中的IMU预积分【SLAM】

世界系下连续时间的IMU积分

$w$代表世界系，$b_k$代表第k帧图像。
在$[t_k,t_{k+1}]$时间段内，有通过加速度和角速度在连续时间下的积分：
$$p_{b_{k+1}}^w=p_{b_k}^w+v_{b_k}^w\Delta t_k+{\iint }_{t\in [t_k,t_{k+1}]}(R_t^w({\hat{a}}_t-b_{a_t}-n_a)-g^w)d{t}^2$$
$$v_{b_{k+1}}^w=v_{b_k}^w+{\int }_{t\in [t_k,t_{k+1}]}(R_t^w({\hat{a}}_t-b_{a_t}-n_a)-g^w)dt$$
$$q_{b_{k+1}}^w=q_{b_k}^w\otimes {\int }_{t\in [t_k,t_{k+1}]}\frac{1}{2}\Omega ({\hat{\omega }}_t-b_{{\omega }_t}-n_{\omega })q_t^{b_k}dt$$

其中$\Delta t_k=t_{k+1}-t_k$,$\Omega (\cdot )$是四元数乘法右乘矩阵的虚数部分，$\Omega (\omega )=\left[\begin{array}{cc}-[\omega {]}_{\times }& \omega \\ -{\omega }^T& 0\end{array}\right]$。

IMU预积分的形成

对上一部分的三个积分公式转换坐标系就可以形成预积分的定义式：
$$R_w^{b_k}{p}_{b_{k+1}}^w=R_w^{b_k}(p_{b_k}^w+v_{b_k}^w\Delta t_k-\frac{1}{2}{g}^w\Delta t_k^2)+{\alpha }_{b_{k+1}}^{b_k}$$
$$R_w^{b_k}{v}_{b_{k+1}}^w=R_w^{b_k}(v_{b_k}^w-g^w\Delta t_k)+{\beta }_{b_{k+1}}^{b_k}$$
$$q_w^{b_k}\otimes q_{b_{k+1}}^w={\gamma }_{b_{k+1}}^{b_k}$$
其中$\alpha 、\beta 、\gamma$就是IMU预积分量。
$${\alpha }_{b_{k+1}}^{b_k}={\iint }_{t\in [t_k,t_{k+1}]}{R}_t^{b_k}({\hat{a}}_t-b_{a_t}-n_a)d{t}^2$$
$${\beta }_{b_{k+1}}^{b_k}={\int }_{t\in [t_k,t_{k+1}]}{R}_t^{b_k}({\hat{a}}_t-b_{a_t}-n_a)dt$$
$${\gamma }_{b_{k+1}}^{b_k}={\int }_{t\in [t_k,t_{k+1}]}\frac{1}{2}\Omega ({\hat{\omega }}_t-b_{{\omega }_t}-n_{\omega }){\gamma }_t^{b_k}dt$$
可以看出预积分量只和状态量中的零偏有关，如果零偏更新了，就用预积分对零偏的一阶雅可比更新预积分。

离散时间下的IMU预积分递推公式

实际IMU信息是离散的，所以要得到离散时间下的IMU预积分公式，可以用Euler、中值积分、RK4进行数值积分求IMU预积分。
离散时间下IMU预积分量的递推公式为：
$${\hat{\alpha }}_{i+1}^{b_k}={\hat{\alpha }}_i^{b_k}+{\hat{\beta }}_i^{b_k}\delta t+\frac{1}{2}R({\hat{\gamma }}_i^{b_k})({\hat{a}}_i-b_{a_i})\delta t^2$$
$${\hat{\beta }}_{i+1}^{b_k}={\hat{\beta }}_i^{b_k}+R({\hat{\gamma }}_i^{b_k})({\hat{a}}_i-b_{a_i})\delta t$$
$${\hat{\gamma }}_{i+1}^{b_k}={\hat{\gamma }}_i^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}({\hat{\omega }}_i-b_{{\omega }_i})\delta t\end{array}\right]$$
其中$i$表示第$i$帧imu信息。
如果用中值积分方法，预积分更新顺序为$\gamma 、\alpha 、\beta$。

误差卡尔曼（ESKF）求预积分量的协方差矩阵

误差卡尔曼中状态量为包含$\alpha 、\beta 、\theta 、b_{a_t}、b_{{\omega }_t}$共15维的状态变量。其中旋转姿态用$\theta$而不再用$\gamma$表示，这样做的原因是四元数表示旋转本身是过参数化的，另外用旋转向量表示求雅可比矩阵方便一些，符合平常的计算习惯，平常不用旋转向量的原因是旋转向量具有周期性，但是在误差卡尔曼中不存在周期性这个问题，误差都是小量。

于是按照误差卡尔曼的推导方式可以得到连续时间下误差状态量的导数公式：
$$\delta {\dot{x}}_t^{b_k}=\left[\begin{array}{c}\delta {\dot{\alpha }}_t^{b_k}\\ \delta {\dot{\beta }}_t^{b_k}\\ \delta {\dot{\theta }}_t^{b_k}\\ \delta {\dot{b}}_{a_t}\\ \delta {\dot{b}}_{{\omega }_t}\end{array}\right]=\left[\begin{array}{ccccc}0& I& 0& 0& 0\\ 0& 0& -R_t^{b_k}[{\hat{a}}_t-b_{a_t}{]}_{\times }& -R_t^{b_k}& 0\\ 0& 0& -[\hat{\omega }-b_{{\omega }_t}{]}_{\times }& 0& -I\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\delta {\alpha }_t^{b_k}\\ \delta {\beta }_t^{b_k}\\ \delta {\theta }_t^{b_k}\\ \delta b_{a_t}\\ \delta b_{{\omega }_t}\end{array}\right]+\left[\begin{array}{cccc}0& 0& 0& 0\\ -R_t^{b_k}& 0& 0& 0\\ 0& -I& 0& 0\\ 0& 0& I& 0\\ 0& 0& 0& I\end{array}\right]\left[\begin{array}{c}{n}_a\\ n_{\omega }\\ n_{b_a}\\ n_{b_{\omega }}\end{array}\right]=F_t\delta x_t^{b_k}+G_t{n}_t$$
通过这个连续时间的导数公式，可以推出离散时间下的误差状态变量的递推公式：
$$\delta x_{i+1}^{b_k}=\delta x_i^{b_k}+(F_t\delta x_i^{b_k}+G_t{n}_t)\delta t=(I+F_t\delta t)\delta x_k+G_t\delta t\cdot n_t$$
注意！这里离散化用的是中值积分，所以$n_t$变成了18维的向量，包含了$n_{a_i}、n_{{\omega }_i}、n_{a_{i+1}}、n_{{\omega }_{i+1}}、n_{b_a}、n_{b_{\omega }}$。
最终结果太复杂了，直接截图，下面图片是cvlife课程的资料：

图片中和本篇博客差别在于图片中的$k$表示第$k$帧imu信息，本篇博客为了区别图像帧和imu帧用了$i$表示第$i$帧imu信息。
为了方便说明，将这个大公式写作：
$$\delta x_{i+1}=F\delta x_i+Vn$$

重要！ 误差状态量和实际状态量雅可比矩阵的关系

我们上面推出的是误差状态量$\delta x=\left[\begin{array}{c}\delta \alpha \\ \delta \beta \\ \delta \theta \\ \delta b_a\\ \delta b_{\omega }\end{array}\right]$的递推公式，实际上我们也比较关心状态量$x=\left[\begin{array}{c}\alpha \\ \beta \\ \theta \\ b_a\\ b_{\omega }\end{array}\right]$对状态量$x$的雅可比矩阵，其实二者有很大的关系。

已知状态量的递推关系：
$$x_{i+1}^t=f(x_i^t)$$
这里上标$t$表示实际状态变量，区分于名义状态变量。
按照误差卡尔曼的方式写成名义状态变量和误差状态变量，有：
$$x_{i+1}+\delta x_{i+1}=f(x_i+\delta x_i)$$

然后把$f(\cdot )$一阶泰勒展开：
$$x_{i+1}+\delta x_{i+1}=f(x_i)+J\delta x_i$$
其中，$J=\frac{\partial x_{i+1}^t}{\partial x_i}$。

又因为名义状态变量满足：
$$x_{i+1}=f(x_i)$$
所以有：
$$\delta x_{i+1}=J\delta x_i$$
也就是说，上一部分中误差状态变量递推公式的大F矩阵就是这里实际状态变量对i时刻名义状态变量的雅可比矩阵，即：
$$F=J=\frac{\partial x_{i+1}^t}{\partial x_i}$$

同时这里还有一点，$x_{i+1}^t=x_{i+1}+\delta x_{i+1}$对x_{i}求偏导，其中$x_{i+1}=f(x_i)$，而$\delta x_{i+1}$与$x_i$无关，所以$F=J=\frac{\partial x_{i+1}}{\partial x_i}$。

然后考虑如果要求$i+1$时刻实际状态变量对初始时刻的雅可比矩阵，不妨设$J_i=\frac{\partial x_i}{\partial x}$，$x$代表本次预积分初始时刻的名义状态变量，然后有：
$$J_{i+1}=F{J}_i$$

维护这个雅可比矩阵的主要作用是，其中包含了实际预积分量对零偏的雅可比矩阵，当零偏更新时，可以利用这个雅可比矩阵更新预积分量。

由$\delta x_{i+1}=F\delta x_i+Vn$可知误差状态变量的协方差矩阵更新方式：
$$P_{i+1}=F{P}_i{F}^T+VQ{V}^T$$
其中，$P_i$为$\delta x_i$的协方差矩阵，$Q$为$n$的协方差矩阵。
由于实际状态变量和误差状态变量的关系为：
$$x_i^t=x_i+\delta x_i$$
其中名义状态变量协方差矩阵为0，所以实际状态变量的协方差矩阵等于误差状态变量的协方差矩阵。