VINS-Mono理论学习——IMU预积分 Pre-integration （Jacobian 协方差）

引言

VINS论文的IV-B. IMU Pre-integration介绍了IMU预积分模型，Foster的两篇论文对IMU预积分理论进行详细分析。

传统传统捷联惯性导航的递推算法，是在已知上一时刻的IMU状态量（姿态和速度、位移）情况下，利用IMU测量得到的线加速度和角速度，做积分运算得到当前时刻的状态量。

然而在基于非线性优化的VIO中，各个节点的状态量都是估计值（此处还包括Ba和Bg）。当这些状态量在优化算法中，会不断调整，每次调整都需要在它们之间重新积分，传递IMU测量值。因此提出了IMU预积分，希望对IMU的相对测量进行处理，使它与绝对位姿解耦，或者只要线性运算就可以进行矫正，从而避免绝对位姿被优化时进行重复积分。

所有式子中的旋转采用了右乘形式(from local to global)，可能看起来不习惯。使用右乘形式的好处是，可以直接使用角速度在体坐标系下的坐标，而左乘形式需要将角速度转换到世界坐标系下。

因本人能力有限，本文有大量内容参考了崔华坤的《VINS论文推导及代码解析》、邱笑晨《预积分总结与公式推导》。

IMU模型

IMU测量值包括加速度计得到的线加速度${\hat{a}}_t$和陀螺仪得到的角速度${\hat{w}}_t$ 【论文式(1)】
$${\hat{a}}_t=a_t+b_{a_t}+R_w^t{g}^w+n_a$$

$${\hat{w}}_t=w_t+b_{w_t}+n_w$$

其中 $t$ 下标表示在体坐标系（IMU坐标系）下，并受到加速度偏置$b_a$、陀螺仪偏置$b_w$和附加噪声$n_a、n_w$的影响。线加速度是重力加速度和物体加速度的合矢量。

假设附加噪声为高斯噪声：
$$n_a\sim N(0,{\sigma }_a^2)，n_w\sim N(0,{\sigma }_w^2)$$

加速度计偏置和陀螺仪偏置被建模为随机游走，其导数为高斯性的： 【论文式(2)】
$${\dot{b}}_{a_t}=n_{b_a}，{\dot{b}}_{w_t}=n_{b_w}$$

$$n_{b_a}\sim N(0,{\sigma }_{b_a}^2)，n_{b_w}\sim N(0,{\sigma }_{b_w}^2)$$

对于图像帧k和k+1，体坐标系对应为$b_k$和$b_{k+1}$，位置、速度和方向状态值PVQ可以根据$[t_k,t_{k+1}]$时间间隔内的IMU测量值，在世界坐标系下进行传递：【论文式(3)(4)】
$$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}{q}_t^{b_k}\otimes \left[\begin{array}{c}({\hat{w}}_t-b_{w_t}-n_w)\\ 0\end{array}\right]dt$$

$$=q_{b_k}^w\otimes {\int }_{t\in [t_k,t_{k+1}]}\frac{1}{2}\Omega ({\hat{w}}_t-b_{w_t}-n_w)q_t^{b_k}dt\text{\hspace{1em}(∗)}$$

$$\Omega (\omega )=\left[\begin{array}{cc}-[\omega {]}_{\times }& \omega \\ -{\omega }^T& 0\end{array}\right]，[\omega {]}_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$

$\Delta t_k$是$[t_k,t_{k+1}]$之间的时间间隔，$R_t^w$为t时刻本体坐标系相对于bk时刻本体坐标系的旋转矩阵，$q_t^{b_k}$为用四元数表示的t时刻从体坐标系到世界坐标系的旋转，这里的四元数实部在后，虚部在前。这里的$\Omega (\omega )$表示四元数右乘。

可以看到，坐标系$b_{k+1}$的状态（$p_{b_{k+1}}^w、v_{b_{k+1}}^w、q_{b_{k+1}}^w$）依赖于$b_k$的状态。那么如果直接将体坐标系的状态作为变量放入非线性优化中，在进行后端优化时会迭代更新，这将导致我们需要根据每次迭代后的状态量重新进行状态传递，即需要重新传递IMU状态值，这样就会存在很大的计算量。

---

关于(*)的推导：
这里主要需要知道四元数的左乘右乘以及导数定理（这部分将参考我关于四元数的博客）：
令$q(t)$是一个单位四元数函数，$\omega (t)$ 是由$q(t)$确定的角速度。则$q(t)$的导数为：
$$\dot{q}=\frac{1}{2}\left[\begin{array}{c}\omega \\ 0\end{array}\right]\otimes q（左乘）=\frac{1}{2}q\otimes \left[\begin{array}{c}\omega \\ 0\end{array}\right]（右乘）$$

因此有：
$$q_{b_{k+1}}^w=q_{b_k}^w\otimes q_{b_{k+1}}^{b_k}=q_{b_k}^w\otimes {\int }_{t\in [t_k,t_{k+1}]}{\dot{q}}_t^{b_k}dt=q_{b_k}^w\otimes {\int }_{t\in [t_k,t_{k+1}]}\frac{1}{2}{q}_t^{b_k}\otimes \left[\begin{array}{c}{\omega }_t^{b_k}\\ 0\end{array}\right]dt$$

$$=q_{b_k}^w\otimes {\int }_{t\in [t_k,t_{k+1}]}\frac{1}{2}{q}_t^{b_k}\otimes \left[\begin{array}{c}({\hat{w}}_t-b_{w_t}-n_w)\\ 0\end{array}\right]dt$$

---

离散形式

下面还给出基于中值积分的离散形式公式，这在void Estimator::processIMU()函数中得以实现。 作用是将IMU积分出来第j时刻PVQ作为第j帧图像的优化初始值。

$$p_{b_{k+1}}^w=p_{b_k}^w+v_{b_k}^w\Delta t_k+\frac{1}{2}\overline{{\hat{a}}_t}\delta t^2$$

$$v_{b_{k+1}}^w=v_{b_k}^w+\overline{{\hat{a}}_t}\delta t$$

$$q_{b_{k+1}}^w=q_{b_k}^w\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\bar{\hat{w}}}_t\delta t\end{array}\right]$$
$$\overline{{\hat{a}}_t}=\frac{1}{2}[q_i({\hat{a}}_t-b_{a_i})-g^w+q_{i+1}({\hat{a}}_{t+1}-b_{a_i})-g^w]$$

$$\overline{{\hat{w}}_t}=\frac{1}{2}({\hat{w}}_t+{\hat{w}}_{t+1})-b_{w_i}$$

---

IMU预积分

IMU预积分的思想就是将参考坐标系从世界坐标系$w$调整为第$k$帧的体坐标系$b_k$，即同时乘以$R_w^{b_k}$：【论文式(5)(6)】

$$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 }_{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{w}}_t-b_{w_t}-n_w){\gamma }_t^{b_k}dt$$

此时的积分结果${\alpha }_{b_{k+1}}^{b_k}、{\beta }_{b_{k+1}}^{b_k}、{\gamma }_{b_{k+1}}^{b_k}$可以理解为$b_{k+1}$对$b_k$的相对运动量，$b_k$的状态改变并不会对其产生影响，因此将其作为非线性优化变量，可以避免状态的重复传递。

注意，这是在假设IMU的偏置$b_a、b_w$已经确定的情况下，实际上偏置也是需要优化的变量，那么在每次迭代时，$b_a、b_w$发生改变，得重新根据公式求得所有帧之间的IMU预积分。

当偏置变换很小时，可以将${\alpha }_{b_{k+1}}^{b_k}、{\beta }_{b_{k+1}}^{b_k}、{\gamma }_{b_{k+1}}^{b_k}$ 按其对偏置的一阶近似来调整，否则就进行重新传递。[论文式(12)]（这部分暂时只是抛出一个概念，后面会讲为什么这样写）

$${\alpha }_{b_{k+1}}^{b_k}\approx {\hat{\alpha }}_{b_{k+1}}^{b_k}+J_{b_a}^{\alpha }\delta b_a+J_{b_w}^{\alpha }\delta b_w$$

$${\beta }_{b_{k+1}}^{b_k}\approx {\hat{\beta }}_{b_{k+1}}^{b_k}+J_{b_a}^{\beta }\delta b_a+J_{b_w}^{\beta }\delta b_w$$

$${\gamma }_{b_{k+1}}^{b_k}\approx {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_w}^{\gamma }\delta b_w\end{array}\right]$$

---

离散形式

接下来讨论一下对于离散时间的具体实现。这里需要提出的是在VINS论文中使用的是欧拉积分，而代码中使用的是中值积分。

欧拉法给出第i时刻与第i+1时刻的变量估计值的关系为：【论文式(7)】

$${\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 {\hat{\gamma }}_{i+1}^i={\hat{\gamma }}_i^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}({\hat{w}}_i-b_{w_i})\delta t\end{array}\right]$$

而代码中采用的基于中值法的IMU预积分公式，在Estimator::processIMU()函数中的IntergrationBase::push_back()中得以实现。

$${\hat{\alpha }}_{i+1}^{b_k}={\hat{\alpha }}_i^{b_k}+{\hat{\beta }}_i^{b_k}\delta t+\frac{1}{2}{\bar{\hat{a}}}_i\delta t^2$$

$${\hat{\beta }}_{i+1}^{b_k}={\hat{\beta }}_i^{b_k}+{\bar{\hat{a}}}_i\delta t$$

$${\hat{\gamma }}_{i+1}^{b_k}={\hat{\gamma }}_i^{b_k}\otimes {\hat{\gamma }}_{i+1}^i={\hat{\gamma }}_i^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\bar{\hat{w}}}_i\delta t\end{array}\right]$$

$${\bar{\hat{a}}}_i=\frac{1}{2}[q_i({\hat{a}}_i-b_{a_i})+q_{i+1}({\hat{a}}_{i+1}-b_{a_i})]$$

$${\bar{\hat{w}}}_i=\frac{1}{2}({\hat{w}}_i+{\hat{w}}_{i+1})-b_{w_i}$$

初始状态${\alpha }_{b_k}^{b_k}、{\beta }_{b_k}^{b_k}为0，{\gamma }_{b_k}^{b_k}$为单位四元数，$n_a、n_w$被视为0，$i$为在$[k,k+1]$中IMU测量值的某一时刻，$\delta t$为IMU测量值i和i+1之间的时间间隔。

---

误差递推方程、协方差和雅可比矩阵

目前我们已经完成了IMU预积分测量值的推导，而要将IMU预积分运用到非线性优化中，需要建立线性高斯误差状态递推方程，由线性高斯系统的协方差，推导方程协方差矩阵，并求解对应的雅可比矩阵。

首先由于四元数${\gamma }_t^{b_k}$被过参数化，我们将其误差项定义为围绕其均值的扰动：【论文式(8)】
$${\gamma }_t^{b_k}\approx {\hat{\gamma }}_t^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_t^{b_k}\end{array}\right]$$

我们给出在t时刻误差项的线性化递推方程为：【论文式(9)】
$$\begin{gathered} \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}}_{w_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{w}}_t-b_{w_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_{w_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_w\\ n_{b_a}\\ n_{b_w}\end{array}\right] \\ =F_t\delta z_t^{b_k}+G_t{n}_t\text{\hspace{1em}(*)} \end{gathered}$$

可以简写为：
$$\delta {\dot{z}}_t^{b_k}=F_t\delta z_t^{b_k}+G_t{n}_t$$
其中：$F_t$是15X15，$G_t$是15X12，$\delta z_t^{b_k}$是15X1，$n_t$是12X1。

---

关于(*)的推导：

先把之前推导的预积分再写一遍，看的更清晰：

$${\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{w}}_t-b_{w_t}-n_w){\gamma }_t^{b_k}dt$$

1）首先对于$\delta {\dot{\alpha }}_t^{b_k}$、$\delta {\dot{b}}_{a_t}$、$\delta {\dot{b}}_{w_t}$，显然根据定义有：

$$\delta {\dot{\alpha }}_t^{b_k}={\hat{\dot{\alpha }}}_t^{b_k}-{\dot{\alpha }}_t^{b_k}={\hat{\beta }}_t^{b_k}-{\beta }_t^{b_k}=\delta {\beta }_t^{b_k}$$

$$\delta {\dot{b}}_{a_t}={\dot{b}}_{a_t}-0=n_{b_a}$$

$$\delta {\dot{b}}_{w_t}={\dot{b}}_{w_t}-0=n_{b_w}$$

2）然后是$\delta {\dot{\beta }}_t^{b_k}$：

${\dot{\beta }}_t^{b_k}$的理论值，即不考虑存在噪声$n_a$和$n_{b_a}$时应为：${\dot{\beta }}_t^{b_k}=R_t^{b_k}({\hat{a}}_t-b_{a_t})$

${\dot{\beta }}_t^{b_k}$的实际测量值，即考虑噪声时应为：

$${\hat{\dot{\beta }}}_t^{b_k}={\hat{R}}_t^{b_k}({\hat{a}}_t-{\hat{b}}_{a_t}-n_a)=R_t^{b_k}exp([\delta \theta {]}_{\times })({\hat{a}}_t-n_a-b_{a_t}-\delta b_{a_t})$$

$$=R_t^{b_k}(1+[\delta \theta {]}_{\times })({\hat{a}}_t-n_a-b_{a_t}-\delta b_{a_t})$$

$$=R_t^{b_k}({\hat{a}}_t-n_a-b_{a_t}-\delta b_{a_t}+[\delta \theta {]}_{\times }({\hat{a}}_t-b_{a_t}))$$

$$=R_t^{b_k}({\hat{a}}_t-n_a-b_{a_t}-\delta b_{a_t}-[{\hat{a}}_t-b_{a_t}{]}_{\times }\delta \theta )$$

则：

$$\delta {\dot{\beta }}_t^{b_k}={\hat{\dot{\beta }}}_t^{b_k}-{\dot{\beta }}_t^{b_k}=-R_t^{b_k}[{\hat{a}}_t-b_{a_t}{]}_{\times }\delta \theta -R_t^{b_k}\delta b_{a_t}-R_t^{b_k}{n}_a$$

证毕。

3）接下来是$\delta {\dot{\theta }}_t^{b_k}$，先推个$\delta {\dot{q}}_t^{b_k}$：

${\dot{q}}_t^{b_k}$的理论值，即不考虑噪声$n_w$和$n_{b_w}$：
$${\dot{q}}_t^{b_k}=\frac{1}{2}\Omega ({\hat{w}}_t-b_{w_t})q_t^{b_k}=\frac{1}{2}{q}_t^{b_k}\otimes \left[\begin{array}{c}({\hat{w}}_t-b_{w_t})\\ 0\end{array}\right]$$

${\dot{q}}_t^{b_k}$的真实测量值：$$\begin{gathered} {\hat{\dot{q}}}_t^{b_k}=\frac{1}{2}{\hat{q}}_t^{b_k}\otimes \left[\begin{array}{c}{\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\\ 0\end{array}\right] \\ =\frac{1}{2}{q}_t^{b_k}\otimes \delta q_t^{b_k}\otimes \left[\begin{array}{c}{\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\\ 0\end{array}\right] \end{gathered}$$

根据导数性质，有：
$${\hat{\dot{q}}}_t^{b_k}=(q_t^{b_k}\dot{\otimes }\delta q_t^{b_k})={\dot{q}}_t^{b_k}\otimes \delta q_t^{b_k}+q_t^{b_k}\otimes \dot{\delta q_t^{b_k}}=\frac{1}{2}{q}_t^{b_k}\otimes \left[\begin{array}{c}({\hat{w}}_t-b_{w_t})\\ 0\end{array}\right]\otimes \delta q_t^{b_k}+q_t^{b_k}\otimes \dot{\delta q_t^{b_k}}$$

即得到等式：

$$\frac{1}{2}{q}_t^{b_k}\otimes \delta q_t^{b_k}\otimes \left[\begin{array}{c}{\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\\ 0\end{array}\right]=\frac{1}{2}{q}_t^{b_k}\otimes \left[\begin{array}{c}({\hat{w}}_t-b_{w_t})\\ 0\end{array}\right]\otimes \delta q_t^{b_k}+q_t^{b_k}\otimes \dot{\delta q_t^{b_k}}$$

$$2\dot{\delta q_t^{b_k}}=\delta q_t^{b_k}\otimes \left[\begin{array}{c}{\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\\ 0\end{array}\right]-\left[\begin{array}{c}({\hat{w}}_t-b_{w_t})\\ 0\end{array}\right]\otimes \delta q_t^{b_k}$$

$$2\dot{\delta q_t^{b_k}}=\mathcal{R}(\left[\begin{array}{c}{\hat{w}}_t-b_{w_t}-n_w-\delta b_{w_t}\\ 0\end{array}\right])\delta q_t^{b_k}-\mathcal{L}(\left[\begin{array}{c}({\hat{w}}_t-b_{w_t})\\ 0\end{array}\right])\delta q_t^{b_k}$$

$$2\dot{\delta q_t^{b_k}}=\left[\begin{array}{c}\dot{\delta {\theta }_t^{b_k}}\\ 0\end{array}\right]=\left[\begin{array}{cc}-[2{\hat{w}}_t-2b_{w_t}-n_w-\delta b_{w_t}{]}_{\times }& -n_w-\delta b_{w_t}\\ (n_w+\delta b_{w_t}{)}^T& 0\end{array}\right]\left[\begin{array}{c}\frac{1}{2}\delta {\theta }_t^{b_k}\\ 1\end{array}\right]$$

$$\begin{gathered} \dot{\delta {\theta }_t^{b_k}}=-[2{\hat{w}}_t-2b_{w_t}-n_w-\delta b_{w_t}{]}_{\times }\frac{1}{2}\delta {\theta }_t^{b_k}-n_w-\delta b_{w_t} \\ \approx -[{\hat{w}}_t-b_{w_t}{]}_{\times }\delta {\theta }_t^{b_k}-n_w-\delta b_{w_t} \end{gathered}$$

证毕。

注：这部分的推导完全参考了崔博的论文推导。

---

那我们继续，上面已经推到了：$\delta {\dot{z}}_t^{b_k}=F_t\delta z_t^{b_k}+G_t{n}_t$
根据导数定义有：
$$\delta {\dot{z}}_t^{b_k}=\underset{\delta t\to 0}{\lim }\frac{\delta z_{t+\delta t}^{b_k}-\delta z_t^{b_k}}{\delta t}$$

即：
$$\delta z_{t+\delta t}^{b_k}=\delta z_t^{b_k}+\delta {\dot{z}}_t^{b_k}\delta t=\delta z_t^{b_k}+(F_t\delta z_t^{b_k}+G_t{n}_t)\delta t=(I+F_t\delta t)\delta z_t^{b_k}+G_t\delta t{n}_t$$

令$F=(I+F_t\delta t)$，$V=G_t\delta t$，有：
$$\delta z_{t+\delta t}^{b_k}=F\delta z_t^{b_k}+V{n}_t$$

由此我们得到了该非线性系统的线性化递推方程，那么便可以根据当前时刻的值，预测下一时刻的协方差：【论文式(10)】
$$P_{t+\delta t}^{b_k}=(I+F_t\delta t)P_t^{b_k}(I+F_t\delta t{)}^T+(G_t\delta t)Q(G_t\delta t{)}^T$$

其中初始协方差$P_{b_k}^{b_k}=0$，Q代表噪声项的对角协方差矩阵：
$$Q^{12\times 12}=\left[\begin{array}{cccc}{\sigma }_a^2& 0& 0& 0\\ 0& {\sigma }_w^2& 0& 0\\ 0& 0& {\sigma }_{b_a}^2& 0\\ 0& 0& 0& {\sigma }_{b_w}^2\end{array}\right]$$

同时也可以根据递推方程得到对应的Jacobian迭代公式：【论文式11】
$$J_{t+\delta t}=(I+F_t\delta t)J_t$$

其中初始Jacobian为：$J_{b_k}=I$。

---

离散形式

根据连续形式的增量误差递推方程，同理我们可以推导出离散形式的方程，这里先直接给出结果。这在vins_estimator/src/factor/integration_base.h中的void IntegrationBase::midPointIntegration()函数中得以实现。

$$\left[\begin{array}{c}\delta {\alpha }_{k+1}\\ \delta {\theta }_{k+1}\\ \delta {\beta }_{k+1}\\ \delta b_{a_{k+1}}\\ \delta b_{w_{k+1}}\end{array}\right]=\left[\begin{array}{ccccc}I& f_{01}& \delta t& f_{03}& f_{04}\\ 0& f_{11}& 0& 0& -\delta t\\ 0& f_{21}& I& f_{23}& f_{24}\\ 0& 0& 0& I& 0\\ 0& 0& 0& 0& I\end{array}\right]\left[\begin{array}{c}\delta {\alpha }_k\\ \delta {\theta }_k\\ \delta {\beta }_k\\ \delta b_{a_k}\\ \delta b_{w_k}\end{array}\right]+\left[\begin{array}{cccccc}{v}_{00}& v_{01}& v_{02}& v_{03}& 0& 0\\ 0& -\frac{1}{2}\delta t& 0& -\frac{1}{2}\delta t& 0& 0\\ -\frac{1}{2}{R}_k\delta t& v_{21}& -\frac{1}{2}{R}_{k+1}\delta t& v_{23}& 0& 0\\ 0& 0& 0& 0& \delta t& 0\\ 0& 0& 0& 0& 0& \delta t\end{array}\right]\left[\begin{array}{c}{n}_{a_k}\\ n_{w_k}\\ n_{a_{k+1}}\\ n_{w_{k+1}}\\ n_{b_a}\\ n_{b_w}\end{array}\right]$$

简写为：
$${\delta z_{k+1}}^{15\times 1}=F^{15\times 15}{\delta z_k}^{15\times 1}+V^{15\times 18}{n}^{18\times 1}\text{\hspace{1em}(*)}$$

Jacobian的迭代公式为：
$$J_{k+1}=F{J}_k，J_0=I$$

协方差迭代公式为：
$$P_{k+1}=F{P}_k{F}^T+VQ{V}^T，P_0=0$$

噪声项的对角协方差矩阵Q为：$Q^{18\times 18}=({\sigma }_a^2,{\sigma }_w^2,{\sigma }_a^2,{\sigma }_w^2,{\sigma }_{b_a}^2,{\sigma }_{b_w}^2)$。

---

关于(*)的推导：

1）对于$\delta b_{a_{k+1}}、\delta b_{w_{k+1}}$，根据偏置模型定义显然有：

$$\delta b_{a_{k+1}}=\delta b_{a_k}+n_{b_a}\delta t，\delta b_{w_{k+1}}=\delta b_{w_k}+n_{b_w}\delta t$$

2）对于$\delta {\theta }_{k+1}$：
之前已经得到了角度误差导数的连续形式：
$$\dot{\delta {\theta }_t^{b_k}}=-[{\hat{w}}_t-b_{w_t}{]}_{\times }\delta {\theta }_t^{b_k}-n_w-\delta b_{w_t}$$

那么其中值积分的离散形式为：
$$\dot{\delta {\theta }_k}=-[\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}{]}_{\times }\delta {\theta }_k-\frac{n_{w_k}+n_{w_{k+1}}}{2}-\delta b_{w_k}$$

根据导数定义可得下一时刻的误差为：
$$\delta {\theta }_{k+1}=\delta {\theta }_k+\dot{\delta {\theta }_k}\delta t=(I-[\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}{]}_{\times }\delta t)\delta {\theta }_k-\frac{n_{w_k}+n_{w_{k+1}}}{2}\delta t-\delta b_{w_k}\delta t$$

整理得
$$\delta {\theta }_{k+1}=(I-[\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}{]}_{\times }\delta t)\delta {\theta }_k-\delta t\delta b_{w_k}-\frac{\delta t}{2}{n}_{w_k}-\frac{\delta t}{2}{n}_{w_{k+1}}=f_{11}\delta {\theta }_k-\delta t\delta b_{w_k}-\frac{\delta t}{2}{n}_{w_k}-\frac{\delta t}{2}{n}_{w_{k+1}}$$

得

$$f_{11}=(I-[\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}{]}_{\times }\delta t)$$

3）对于$\delta {\beta }_{k+1}$：
之前已经得到了速度误差导数的连续形式：

$$\delta {\dot{\beta }}_t^{b_k}=-R_t^{b_k}[{\hat{a}}_t-b_{a_t}{]}_{\times }\delta \theta -R_t^{b_k}\delta b_{a_t}-R_t^{b_k}{n}_a$$

那么其中值积分的离散形式为：

$$\delta {\dot{\beta }}_k=-\frac{1}{2}{R}_k[{\hat{a}}_k-b_{a_k}{]}_{\times }\delta {\theta }_k-\frac{1}{2}{R}_{k+1}[{\hat{a}}_{k+1}-b_{a_k}{]}_{\times }\delta {\theta }_{k+1}-\frac{1}{2}(R_k+R_{k+1})\delta b_{a_k}-\frac{1}{2}{R}_k{n}_{a_k}-\frac{1}{2}{R}_{k+1}{n}_{a_{k+1}}$$

把之前的$\delta {\theta }_{k+1}$代进去有：

δ β ˙ k = { − 1 2 R k [ a ^ k − b a k ] × − 1 2 R k + 1 [ a ^ k + 1 − b a k ] × ( I − [ w ^ k + w ^ k + 1 2 − b w k ] × δ t ) } δ θ k + 1 4 R k + 1 [ a ^ k + 1 − b a k ] × δ t ( n w k + n w k + 1 ) + 1 2 R k + 1 [ a ^ k + 1 − b a k ] × δ t δ b w k − 1 2 ( R k + R k + 1 ) δ b a k − 1 2 R k n a k − 1 2 R k + 1 n a k + 1 \delta \dot \beta_{k} =\{-\frac{1}{2}R_{k}[\hat a_k- b_{a_k}]_{\times}-\frac{1}{2}R_{k+1}[\hat a_{k+1}- b_{a_{k}}]_{\times}(I-[\frac{\hat w_k+\hat w_{k+1}}{2}-b_{w_k}]_{\times}\delta t) \}\delta \theta_k\\ +\frac{1}{4}R_{k+1}[\hat a_{k+1}- b_{a_{k}}]_{\times}\delta t(n_{w_k}+n_{w_{k+1}})+\frac{1}{2}R_{k+1}[\hat a_{k+1}- b_{a_{k}}]_{\times}\delta t \delta b_{w_k}\\ -\frac{1}{2}(R_k+R_{k+1}) \delta b_{a_k} -\frac{1}{2}R_k n_{a_k}-\frac{1}{2}R_{k+1} n_{a_{k+1}} δβ˙​k​={−21​Rk​[a^k​−bak​​]×​−21​Rk+1​[a^k+1​−bak​​]×​(I−[2w^k​+w^k+1​​−bwk​​]×​δt)}δθk​+41​Rk+1​[a^k+1​−bak​​]×​δt(nwk​​+nwk+1​​)+21​Rk+1​[a^k+1​−bak​​]×​δtδbwk​​−21​(Rk​+Rk+1​)δbak​​−21​Rk​nak​​−21​Rk+1​nak+1​​

根据导数定义可得下一时刻的误差为：
δ β k + 1 = δ β k + δ β k ˙ δ t = { − 1 2 R k [ a ^ k − b a k ] × − 1 2 R k + 1 [ a ^ k + 1 − b a k ] × ( I − [ w ^ k + w ^ k + 1 2 − b w k ] × δ t ) } δ t δ θ k + δ β k + { 1 4 R k + 1 [ a ^ k + 1 − b a k ] × δ t ( n w k + n w k + 1 ) + 1 2 R k + 1 [ a ^ k + 1 − b a k ] × δ t δ b w k − 1 2 ( R k + R k + 1 ) δ b a k − 1 2 R k n a k − 1 2 R k + 1 n a k + 1 } δ t \delta \beta_{{k+1}} =\delta \beta_k+\dot{\delta \beta_{k}} \delta t=\{-\frac{1}{2}R_{k}[\hat a_k- b_{a_k}]_{\times}-\frac{1}{2}R_{k+1}[\hat a_{k+1}- b_{a_{k}}]_{\times}(I-[\frac{\hat w_k+\hat w_{k+1}}{2}-b_{w_k}]_{\times}\delta t) \}\delta t \delta \theta_k\\ +\delta \beta_k+\{ \frac{1}{4}R_{k+1}[\hat a_{k+1}- b_{a_{k}}]_{\times}\delta t(n_{w_k}+n_{w_{k+1}}) +\frac{1}{2}R_{k+1}[\hat a_{k+1}- b_{a_{k}}]_{\times}\delta t \delta b_{w_k}\\ -\frac{1}{2}(R_k+R_{k+1}) \delta b_{a_k}-\frac{1}{2}R_k n_{a_k}-\frac{1}{2}R_{k+1} n_{a_{k+1}}\}\delta t δβk+1​=δβk​+δβk​˙​δt={−21​Rk​[a^k​−bak​​]×​−21​Rk+1​[a^k+1​−bak​​]×​(I−[2w^k​+w^k+1​​−bwk​​]×​δt)}δtδθk​+δβk​+{41​Rk+1​[a^k+1​−bak​​]×​δt(nwk​​+nwk+1​​)+21​Rk+1​[a^k+1​−bak​​]×​δtδbwk​​−21​(Rk​+Rk+1​)δbak​​−21​Rk​nak​​−21​Rk+1​nak+1​​}δt

整理后得到：

$$\begin{gathered} \delta {\beta }_{k+1}=f_{21}\delta {\theta }_k+\delta {\beta }_k-\frac{1}{2}(R_k+R_{k+1})\delta t\delta b_{a_k}+f_{24}\delta b_{w_k} \\ -\frac{1}{2}{R}_k\delta t{n}_{a_k}-\frac{1}{2}{R}_{k+1}\delta t{n}_{a_{k+1}}+v_{21}{n}_{w_k}+v_{23}{n}_{w_{k+1}} \end{gathered}$$

其中：
$$f_{21}=-\frac{1}{2}{R}_k[{\hat{a}}_k-b_{a_k}{]}_{\times }\delta t-\frac{1}{2}{R}_{k+1}[{\hat{a}}_{k+1}-b_{a_k}{]}_{\times }(I-[\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}{]}_{\times }\delta t)\delta t$$

$$f_{24}=\frac{1}{2}{R}_{k+1}[{\hat{a}}_{k+1}-b_{a_k}{]}_{\times }\delta t^2$$

$$v_{21}=v_{23}=\frac{1}{4}{R}_{k+1}[{\hat{a}}_{k+1}-b_{a_k}{]}_{\times }\delta t^2$$

证毕。

4）对于$\delta {\alpha }_{k+1}$：

之前已经得到了速度误差导数的连续形式：

$$\delta {\dot{\alpha }}_t^{b_k}={\hat{\dot{\alpha }}}_t^{b_k}-{\dot{\alpha }}_t^{b_k}={\hat{\beta }}_t^{b_k}-{\beta }_t^{b_k}=\delta {\beta }_t^{b_k}$$

则离散形式为：
$$\begin{gathered} \delta {\dot{\alpha }}_k=\frac{1}{2}\delta {\beta }_k+\frac{1}{2}\delta {\beta }_{k+1} \\ ={\beta }_k+\frac{1}{2}{f}_{21}\delta {\theta }_k-\frac{1}{4}(R_k+R_{k+1})\delta t\delta b_{a_k}+\frac{1}{2}{f}_{24}\delta b_{w_k} \\ -\frac{1}{4}{R}_k\delta t{n}_{a_k}-\frac{1}{4}{R}_{k+1}\delta t{n}_{a_{k+1}}+\frac{1}{2}{v}_{21}{n}_{w_k}+\frac{1}{2}{v}_{23}{n}_{w_{k+1}} \end{gathered}$$

根据导数定义可得下一时刻的误差为：
$$\begin{gathered} \delta {\alpha }_{k+1}=\delta {\alpha }_k+\delta {\dot{\alpha }}_k\delta t \\ =\delta {\alpha }_k+f_{01}\delta {\theta }_k+\delta t\delta {\beta }_k+f_{03}\delta b_{a_k}+f_{04}\delta b_{w_k}+v_{00}{n}_{a_k}+v_{01}{n}_{w_k}+v_{02}{n}_{a_{k+1}}+v_{03}{n}_{w_{k+1}} \end{gathered}$$

其中：
$$f_{01}=\frac{1}{2}\delta t\delta {\theta }_k，f_{03}=-\frac{1}{4}(R_k+R_{k+1})\delta t^2，f_{04}=\frac{1}{2}{f}_{24}\delta t$$

$$v_{00}=\frac{1}{4}{R}_k\delta t^2，v_{01}=v_{03}=\frac{1}{2}{v}_{21}\delta t，v_{02}=\frac{1}{4}{R}_{k+1}\delta t^2$$

证毕。

---

至此IMU的预积分所有内容已全部推导完毕！
最后写一下个人感受：真的非常感谢崔华坤博士的资料！说是自己写一下关于IMU预积分的学习笔记，但写到后面基本就是照着崔博的公式推导手敲一遍（可能是实在写的太好了）。但不得不说，敲完一遍以后豁然开朗！如果你也想学习IMU预积分的话，强烈建议你把所有公式手推一遍。这些公式看起来那么复杂，其实里面的数学要求并不是很高。还有就是如果对四元数相关有所疑问，推荐看《Quaternion kinematics for the error-state Kalman filter》。