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()函数中得以实现。

[ δ α k + 1 δ θ k + 1 δ β k + 1 δ b a k + 1 δ b w k + 1 ] = [ I f 01 δ t f 03 f 04 0 f 11 0 0 − δ t 0 f 21 I f 23 f 24 0 0 0 I 0 0 0 0 0 I ] [ δ α k δ θ k δ β k δ b a k δ b w k ] + [ v 00 v 01 v 02 v 03 0 0 0 − 1 2 δ t 0 − 1 2 δ t 0 0 − 1 2 R k δ t v 21 − 1 2 R k + 1 δ t v 23 0 0 0 0 0 0 δ t 0 0 0 0 0 0 δ t ] [ n a k