VINS-Mono IMU 预积分公式及协方差、雅可比矩阵传递推导

原论文链接：VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
参考：崔华坤-《VINS论文推导及代码解析》与 高翔-《视觉SLAM十四讲》

整理了 VINS-Mono 论文中 关于 IMU 预积分公式及协方差、雅可比矩阵传递的推导。

1. IMU 原始加速度和角加速度

IMU 原始加速度和角加速度（相对于IMU/本体 坐标系）表示如下：
(1)
$$\begin{array}{rl}& {\hat{\mathbf{a}}}_t={\mathbf{a}}_t+{\mathbf{b}}_{a_t}+{\mathbf{R}}_w^t{\mathbf{g}}^w+{\mathbf{n}}_a\\ & {\hat{\mathbf{\omega }}}_t={\mathbf{\omega }}_t+{\mathbf{b}}_{w_t}+{\mathbf{n}}_w\end{array}$$
(1)式中
$$\begin{array}{rl}& {\mathbf{n}}_a\sim \mathcal{N}\left(0,{\sigma }_a^2\right),{\mathbf{n}}_w\sim \mathcal{N}\left(0,{\sigma }_w^2\right)\\ & {\dot{\mathbf{b}}}_{a_t}={\mathbf{n}}_{b_a},{\mathbf{n}}_{b_a}\sim \mathcal{N}\left(0,{\sigma }_{b_a}^2\right)\\ & {\dot{\mathbf{b}}}_{w_t}={\mathbf{n}}_{b_w},{\mathbf{n}}_{b_w}\sim \mathcal{N}\left(0,{\sigma }_{b_w}^2\right)\end{array}$$

2. 当前时刻 PVQ 的连续形式

定义采集到的连续两帧图像为 $b_k$ 和 $b_{k+1}$，则在时间区间 $[t_k,t_{k+1}]$ 内对位置 P，速度 V 和四元数表示的旋转 Q 进行积分的公式如下（得到结果的参考坐标系为世界坐标系）：
(23)
$$\begin{array}{rl}{\mathbf{p}}_{b_{k+1}}^w& ={\mathbf{p}}_{b_k}^w+{\mathbf{v}}_{b_k}^w\Delta t_k+{\iint }_{t\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_t^w\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_{a_t}-{\mathbf{n}}_a\right)-{\mathbf{g}}^w\right)d{t}^2\\ {\mathbf{v}}_{b_{k+1}}^w& ={\mathbf{v}}_{b_k}^w+{\int }_{t\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_t^w\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_{a_t}-{\mathbf{n}}_a\right)-{\mathbf{g}}^w\right)dt\\ {\mathbf{q}}_{b_{k+1}}^w& ={\mathbf{q}}_{b_k}^w\otimes {\int }_{t\in \left[t_k,t_{k+1}\right]}\frac{1}{2}\Omega \left({\hat{\mathbf{\omega }}}_t-{\mathbf{b}}_{w_t}-{\mathbf{n}}_w\right){\mathbf{q}}_t^{b_k}dt\end{array}$$
where
(24)
$$\mathbf{\Omega }(\mathbf{\omega })=\left[\begin{array}{cc}-\lfloor \mathbf{\omega }{\rfloor }_{\times }& \mathbf{\omega }\\ -{\mathbf{\omega }}^T& 0\end{array}\right],\lfloor \mathbf{\omega }{\rfloor }_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$
$R_t^w$ 表示 t 时刻 IMU/本体 坐标系 到 世界坐标系的旋转矩阵；
$q_t^{b_k}$ 表示 t 时刻 IMU/本体 坐标系 相对于采集 $b_k$的时刻的 IMU/本体 坐标系 的旋转（四元数形式）。
$q_{b_k}^w$ 表示 采集$b_k$的时刻的IMU坐标系 相对于 世界坐标系 的旋转（四元数形式）。论文中四元数向量的实部是向量最后一个元素。

四元数的积分涉及到四元数求导、四元数与旋转向量的转换等的知识。
四元数求导推导请看文章后半部分。

3. 两帧之间 PVQ 增量的连续形式

因为上述公式的积分项存在 $R_t^w$ 或者 $q$，IMU状态传递需要知道 $b_k$ 时刻的 PVQ，优化算法每次对状态优化后都需要重新传递 IMU 的测量，降低了处理速度。

所以引出了预积分。
将参考坐标系从世界坐标系改为 $b_k$ 的 IMU/本体 坐标系，Q、V等式两边同时左乘 $R_w^{b_k}$，Q等式左右两边同时左乘 $q_w^{b_k}$：
(25)
R w b k p b k + 1 w = R w b k ( p b k w + v b k w Δ t k − 1 2 g w Δ t k 2 ) + α b k + 1 b k R w b k v b k + 1 w = R w b k ( v b k w − g w Δ t k ) + β b k + 1 b k q w b k ⊗ q b k + 1 w = γ b k + 1 b k \begin{aligned} \mathbf{R}_{w}^{b_{k}} \mathbf{p}_{b_{k+1}}^{w} &=\mathbf{R}_{w}^{b_{k}}\left(\mathbf{p}_{b_{k}}^{w}+\mathbf{v}_{b_{k}}^{w} \Delta t_{k}-\frac{1}{2} \mathbf{g}^{w} \Delta t_{k}^{2}\right)+\alpha_{b_{k+1}}^{b_{k}} \\ \mathbf{R}_{w}^{b_{k}} \mathbf{v}_{b_{k+1}}^{w} &=\mathbf{R}_{w}^{b_{k}}\left(\mathbf{v}_{b_{k}}^{w}-\mathbf{g}^{w} \Delta t_{k}\right)+\boldsymbol{\beta}_{b_{k+1}}^{b_{k}} \\ \mathbf{q}_{w}^{b_{k}} \otimes \mathbf{q}_{b_{k+1}}^{w} &=\boldsymbol{\gamma}_{b_{k+1}}^{b_{k}} \end{aligned} R