VINS-Mono+Fusion源码解析系列（四）：IMU预积分的推导

1. PVQ模型

在t时刻，IMU在世界坐标系下的位置P、速度V、姿态q（PVQ模型）的导数模型如下：
$$\begin{gathered} {\dot{p}}_{b_t}^w=v_t^w \\ {\dot{v}}_t^w=a_t^w \\ {\dot{q}}_{b_t}^w=q_{b_t}^w\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{w}_t^b\end{array}\right] \end{gathered}$$
​ 其中，$p_{b_t}^w$表示t时刻imu在世界坐标系下的位移，$v_t^w$表示t时刻imu在世界坐标系下的速度，$q_{b_t}^w$表示t时刻imu(body)坐标系到世界坐标系之间的姿态。

​ 基于以上PVQ的导数模型，连续时间下，从第i关键帧IMU的PVQ数据得到第j关键帧IMU的PVQ数据过程如下：
$$\begin{gathered} p_{b_j}^w=p_{b_i}^w+v_{b_i}^w\Delta t+\int {\int }_{t\in [i,j]}(q_{b_t}^w{a}_t^b-g_t^w)\delta t^2 \\ {v}_{b_j}^w=v_{b_i}^w+{\int }_{t\in [i,j]}(q_{b_t}^w{a}_t^b-g_t^w)\delta t \\ {q}_{b_j}^w={\int }_{t\in [i,j]}{\dot{q}}_{b_t}^w\delta t={\int }_{t\in [i,j]}{q}_{b_t}^w\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{w}_t^b\end{array}\right]\delta t \end{gathered}$$
​ 其中，根据IMU的数学模型，body坐标系下的加速度$a_t^b={\hat{a}}_t^b-b_{a_t}-n_a$​，角速度$w_t^b={\hat{w}}_t^b-b_{w_t}-n_w$​。转换到世界坐标系下，有：
$$q_{b_t}^w{a}_t^b=a_t^w+g^w\Rightarrow a_t^w=q_{b_t}^w{a}_t^b-g^w$$
​ 由于待求的位移P、速度V、位姿q都是世界坐标系下的，而IMU测量值得到的加速度$a_t^b$与角速度$w_t^b$都是body坐标系下的，因此需要进行如上转换，获取对应量在世界坐标系下的值。

2. 为什么要进行IMU预积分

 由于IMU的频率远远高于相机频率，所以相邻帧图像间存在多个IMU数据，故两关键帧间必然存在多个IMU数据。为了实现相机与IMU之间的同频效果，在世界坐标系中进行BA优化时，只需考虑相机关键帧处的IMU数据，中间时刻的状态量是不会转换到世界坐标系中加入到BA里面进行优化的。而在使用PVQ模型计算关键帧处的IMU数据时，其中的积分项需要使用位姿$q_{b_t}^w$将每个时刻$t\in [i,j]$中body坐标系下的位姿都转换到世界坐标系下来参与积分运算，这样计算量是非常大的。

3. IMU预积分的实现

  q b t w =