VINS-Mono+Fusion源码解析系列（五）：IMU预积分的误差传递

1. 连续时间下IMU预积分误差传递的推导

 首先，给出连续时间下IMU预积分误差$\delta z_t$的传递方程，然后对各预积分量的误差传递形式进行推导：
（1）位置项$\delta {\dot{\alpha }}_t^{b_k}$的推导

​ 真实值下的位置导数等于其对应的真实速度，即${\dot{P}}_t=V_t$，测量值下的位置导数也等于其对应的测量值速度，即

$\dot{P}=V$​。又由于真实值 = 测量值 + 误差项，则有：
$$(P+\Delta P{)}^{,}=(V+\Delta V)\Rightarrow \dot{P}+\Delta \dot{P}=V+\Delta V\Rightarrow \Delta \dot{P}=\Delta V$$
​ 所以，对于预积分量${\alpha }_t^{b_k}$，它的误差项传递形式为：$\delta {\dot{\alpha }}_t^{b_k}={\beta }_t^{b_k}$

（2）零偏项$\delta {\dot{b}}_{a_t}$与$\delta \dot{b_{w_t}}$的推导

​ 在VINS-Mono中，相邻两帧的时间间隔比较短，因此可认为零偏是不变的，即真实值的零偏导数为0，如下：
$$\dot{b_a}=0\dot{b_w}=0$$
代入测量值与误差项，则有：$(b_a+\Delta b_a{)}^{,}=0$，$(b_w+\Delta b_w{)}^{,}=0$
$$\begin{gathered} (b_a+\Delta b_a{)}^{,}=0\Rightarrow {\dot{b}}_a+\delta {\dot{b}}_a=0 \\ (b_w+\Delta b_w{)}^{,}=0\Rightarrow {\dot{b}}_w+\delta {\dot{b}}_w=0 \end{gathered}$$
​ 由于测量值的偏置的导数服从高斯分布，因此${\dot{b}}_a=n_{b_a}$，${\dot{b}}_w=n_{b_w}$。加上正负号后，零偏的导数仍满足高斯分布，故$\delta {\dot{b}}_a=n_{b_a}$，$\delta {\dot{b}}_w=n_{b_w}$，与方程中偏置的导数项一致

（3）速度项$\delta {\dot{\beta }}_t^{b_k}$的推导

​ 根据PVQ导数模型可知，真实值下的速度导数等于其对应的真实加速度，即${\dot{v}}_t=a_t=\hat{a}-b_a$。对于速度而言，由于所求的是在$[b_k,b_{k+1}]$时间内的速度预积分量，因此其中的速度都是相对于$b_k$时刻的速度，故需要通过$R_t^{b_k}$将加速度转换到$b_k$时刻上，即真实值下满足${\dot{v}}_t=R_t^{b_k}(\hat{a}-b_a)$。

​ 将真实值转换为测量值 + 误差值的形式，则有： $$(v+\Delta v{)}^{,}=R\cdot \Delta R(\hat{a}-n_a-(b_a+\Delta b_a))$$，展开如下：
$$\begin{gathered} (v+\Delta v{)}^{,}=\dot{v}+\delta \dot{v}=R\cdot \Delta R[\hat{a}-n_a-(b_a+\Delta b_a)]=R\cdot exp([\delta \theta {]}^{\wedge })[\hat{a}-n_a-(b_a+\Delta b_a)] \\ =R\cdot (I+[\delta \theta {]}^{\wedge })[\hat{a}-n_a-(b_a+\Delta b_a)]=R(\hat{a}-n_a-b_a-\delta b_a)+R[\delta \theta {]}^{\wedge }(\hat{a}-n_a-b_a-\delta b_a) \end{gathered}$$
​ 由于测量值速度满足：$\dot{v}=R(\hat{a}-b_a)$，且$[\delta \theta {]}^{\wedge }$和$n_a+\delta b_a$均为一阶小量，二者相乘后为二阶小量，可忽略，故上式可进一步化简为：
$$\delta \dot{v}=R(-n_a-\delta b_a)+R[\delta \theta {]}^{\wedge }(\hat{a}-b_a)=-R(\hat{a}-b_a{)}^{\wedge }\delta \theta -R{n}_a-R\delta b_a$$
​ 与方程中速度的导数项一致

（4）姿态项$\delta {\dot{\theta }}_t^{b_k}$的推导

​ 根据PVQ导数模型可知，真实值下的旋转四元数导数表达式为：$${\dot{q}}_{b_t}^w=q_{b_t}^w\otimes [\begin{array}{c}0\\ \frac{1}{2}{w}^{}\end{array}$$