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)
$$\begin{array}{rl}{\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)+{\mathbf{\beta }}_{b_{k+1}}^{b_k}\\ {\mathbf{q}}_w^{b_k}\otimes {\mathbf{q}}_{b_{k+1}}^w& ={\mathbf{\gamma }}_{b_{k+1}}^{b_k}\end{array}$$
where
(26)
$$\begin{array}{rl}{\mathbf{\alpha }}_{b_{k+1}}^{b_k}& ={\iint }_{t\in \left[t_k,t_{k+1}\right]}{\mathbf{R}}_t^{b_k}\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_{a_t}-{\mathbf{n}}_a\right)d{t}^2\\ {\mathbf{\beta }}_{b_{k+1}}^{b_k}& ={\int }_{t\in \left[t_k,t_{k+1}\right]}{\mathbf{R}}_t^{b_k}\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_{a_t}-{\mathbf{n}}_a\right)dt\\ {\mathbf{\gamma }}_{b_{k+1}}^{b_k}& ={\int }_{t\in \left[t_k,t_{k+1}\right]}\frac{1}{2}\mathbf{\Omega }\left({\hat{\mathbf{\omega }}}_t-{\mathbf{b}}_{w_t}-{\mathbf{n}}_w\right){\mathbf{\gamma }}_t^{b_k}dt.\end{array}$$
其中 (26) 就是3个预积分项，只有这三个项存在 $\widehat{a_t}$ 或者 $\widehat{w_t}$。

4. 两帧之间 PVQ 增量的零阶保持法离散形式

讨论离散情况。
论文给出了零阶保持离散化方法：
(27)
$$\begin{array}{rl}& {\hat{\mathbf{\alpha }}}_{i+1}^{b_k}={\hat{\mathbf{\alpha }}}_i^{b_k}+{\hat{\mathbf{\beta }}}_i^{b_k}\delta t+\frac{1}{2}\mathbf{R}\left({\hat{\gamma }}_i^{b_k}\right)\left({\hat{\mathbf{a}}}_i-{\mathbf{b}}_{a_i}\right)\delta t^2\\ & {\hat{\mathbf{\beta }}}_{i+1}^{b_k}={\hat{\mathbf{\beta }}}_i^{b_k}+\mathbf{R}\left({\hat{\gamma }}_i^{b_k}\right)\left({\hat{a}}_i-{\mathbf{b}}_{a_i}\right)\delta t\\ & {\hat{\gamma }}_{i+1}^{b_k}={\hat{\gamma }}_i^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\left({\hat{\omega }}_i-{\mathbf{b}}_{w_i}\right)\delta t\end{array}\right]\end{array}$$
其中 i 表示在时间区间 $[t_k,t_{k+1}]$ 内 IMU 的第 i 个测量，$\delta t$ 为 i~i+1之间的时间长度。

5. IMU 误差项导数

IMU 的积分值是有误差的，对误差进行分析。

将 四元数 $\gamma$ 的误差定义为其均值附近的扰动：
(28)
$${\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]$$
$\delta {\theta }_t^{b_k}$ 是 t 时刻 IMU/本体 坐标系 相对于采集 $b_k$的时刻的 IMU/本体 坐标系 的旋转 3D 小扰动。

连续形式 IMU 误差项导数（误差动力学方程）：
(29)
$$\begin{array}{rl}\left[\begin{array}{c}\delta {\dot{\mathbf{\alpha }}}_t^{b_k}\\ \delta {\dot{\mathbf{\beta }}}_t^{b_k}\\ \delta {\dot{\mathbf{\theta }}}_t^{b_k}\\ \delta {\dot{\mathbf{b}}}_{a_t}\\ \delta {\dot{\mathbf{b}}}_{w_t}\end{array}\right]=& \left[\begin{array}{cccccc}0& \mathbf{I}& 0& 0& 0& 0\\ 0& 0& -{\mathbf{R}}_t^{b_k}{\lfloor {\hat{\mathbf{a}}}_t-{\mathbf{b}}_{a_t}\rfloor }_{\times }& -{\mathbf{R}}_t^{b_k}& 0& 0\\ 0& 0& -\lfloor {\hat{\mathbf{\omega }}}_t& {-{\mathbf{b}}_{w_t}\rfloor }_{\times }& 0& -\mathbf{I}\\ 0& 0& 0& 0& 0& 0\\ 0& 0& & 0& 0& 0\end{array}\right]\left[\begin{array}{c}\delta {\mathbf{\alpha }}_t^{b_k}\\ \delta {\mathbf{\beta }}_t^{b_k}\\ \delta {\mathbf{\theta }}_t^{b_k}\\ \delta {\mathbf{b}}_{a_t}\\ \delta {\mathbf{b}}_{w_t}\end{array}\right]+\left[\begin{array}{cccc}0& 0& 0& 0\\ -{\mathbf{R}}_t^{b_k}& 0& 0& 0\\ 0& -\mathbf{I}& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}{\mathbf{n}}_a\\ {\mathbf{n}}_w\\ {\mathbf{n}}_{b_a}\\ {\mathbf{n}}_{b_w}\end{array}\right]={\mathbf{F}}_t\delta {\mathbf{z}}_t^{b_k}+{\mathbf{G}}_t{\mathbf{n}}_t.\end{array}$$

$F_t{}^{15\times 15},G_t{}^{15\times 12},\delta z_t^{b_k^{15\times 1}},n_t{}^{12\times 1}$

对其中 $\delta {\dot{\beta }}_t^{b_k}$ 和 $\delta {\dot{\theta }}_t^{b_k}$ 进行推导。
$\delta {\dot{\beta }}_t^{b_k}$ 的推导：
引入两个概念：true 和 nominal，其中：
true：真实测量值，包含了噪声
nominal：无噪声的理论值
则

其中第二步中 $exp(\delta \theta$^$)$ 是对 $R_t^{b_k}$ 的扰动，涉及到 李群与李代数 方面的知识，《十四讲》中有相关内容。

$\delta {\dot{\beta }}_t^{b_k}$ 简写成$\delta \dot{\beta }$：

$\delta {\dot{\theta }}_t^{b_k}$ 的推导：

类似的，$\delta q$ 是对 $q_t$ 的扰动。

根据导数的性质（乘法的求导法则），又有：


综合上式，可得等式如下（$\delta {\dot{\theta }}_t^{b_k}$ 简写成$\delta \dot{\theta }$：）：

上式左侧也可以写成：

$q=[\hat{k}sin\frac{\theta }{2},cos\frac{\theta }{2}{]}^T\approx [\frac{\theta }{2}，1]$
$\delta \dot{q}=[\frac{\delta \dot{\theta }}{2}，0{]}^T$

6. 噪声协方差矩阵传递公式

将 式(29) 简写成：$\delta {\dot{z}}_t^{b_k}=F_t\delta z_t^{b_k}+G_t{n}_t$
则（均值预测公式）：
$$\begin{array}{rl}\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=\left(\mathrm{I}+F_t\delta t\right)\delta z_t^{b_k}+\left(G_t\delta t\right)n_t\\ & =F\delta z_t^{b_k}+V{n}_t\end{array}$$
上式中 $F=I+F_t\delta t$ ，$V=G_t\delta t$ 。

上式恰好给出了 EKF 一般对非线性系统线性化的过程，这里的意义是表示下一个时刻的 IMU 测量误差与上一个时刻的成线性关系。

零阶保持法中，${\mathbf{F}}_t$ 在积分周期内为常数，所以
$${\mathbf{F}}_d=\exp \left({\mathbf{F}}_t\delta t\right)\approx \mathbf{I}+{\mathbf{F}}_t\delta t$$

由连续形式噪声协方差矩阵：${\mathbf{Q}}_t=\mathrm{diag}\left({\sigma }_a^2,{\sigma }_w^2,{\sigma }_{b_a}^2,{\sigma }_{b_w}^2\right)$
得到离散形式噪声协方差矩阵：
(30)
$$\begin{array}{rl}{\mathbf{Q}}_d& ={\int }_0^{\delta t}{\mathbf{F}}_d(\tau ){\mathbf{G}}_t{\mathbf{Q}}_t{\mathbf{G}}_t^T{\mathbf{F}}_d(\tau {)}^T\\ & =\delta t{\mathbf{F}}_d{\mathbf{G}}_t{\mathbf{Q}}_t{\mathbf{G}}_t^T{\mathbf{F}}_d^T\\ & \approx \delta t{\mathbf{G}}_t{\mathbf{Q}}_t{\mathbf{G}}_t^T\end{array}$$

协方差预测公式：
原论文
(31)
$$\begin{array}{r}{\mathbf{P}}_{t+\delta t}^{b_k}=\left(\mathbf{I}+{\mathbf{F}}_t\delta t\right){\mathbf{P}}_t^{b_k}{\left(\mathbf{I}+{\mathbf{F}}_t\delta t\right)}^T+\delta t{\mathbf{G}}_t{\mathbf{Q}}_t{\mathbf{G}}_t^T,t\in [k,k+1]\end{array}$$
崔华坤论文推导

初始值 ${\mathbf{P}}_{b_k}^{b_k}=0$

7. 误差项的 Jacobian 矩阵传递公式 和 bias 的一阶近似

根据均值预测公式，也可以写出误差项的 Jacobian 矩阵的迭代公式：
$${\mathbf{J}}_{t+\delta t}=\left(\mathbf{I}+{\mathbf{F}}_t\delta t\right){\mathbf{J}}_t,t\in [k,k+1]$$

初始值 ${\mathbf{J}}_{b_k}=\mathbf{I}$

式(26) 关于 bias 的一阶近似如下：
(33)
$$\begin{array}{rl}{\mathbf{\alpha }}_{b_{k+1}}^{b_k}& \approx {\hat{\mathbf{\alpha }}}_{b_{k+1}}^{b_k}+{\mathbf{J}}_{b_a}^{\alpha }\delta {\mathbf{b}}_{a_k}+{\mathbf{J}}_{b_w}^{\alpha }\delta {\mathbf{b}}_{w_k}\\ {\mathbf{\beta }}_{b_{k+1}}^{b_k}& \approx {\hat{\mathbf{\beta }}}_{b_{k+1}}^{b_k}+{\mathbf{J}}_{b_a}^{\beta }\delta {\mathbf{b}}_{a_k}+{\mathbf{J}}_{b_w}^{\beta }\delta {\mathbf{b}}_{w_k}\\ {\gamma }_{b_{k+1}}^{b_k}& \approx {\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_w}^{\gamma }\delta {\mathbf{b}}_{w_k}\end{array}\right]\end{array}$$

其中 ${\mathbf{J}}_{b_a}^{\alpha }$ 是 ${\mathbf{J}}_{b_{k+1}}$ 的关于 $\delta {\mathbf{\alpha }}_{b_{k+1}}^{b_k}/\delta {\mathbf{b}}_{a_k}$ 的子块。
${\mathbf{J}}_{b_w}^{\alpha },{\mathbf{J}}_{b_a}^{\beta },{\mathbf{J}}_{b_w}^{\beta },{\mathbf{J}}_{b_w}^{\gamma }$ 类似。

当 bias 被优化改变时，使用上式调整预积分而不是重新积分。

8. 协方差为 ${\mathbf{P}}_{b_{k+1}}^{b_k}$ 下 IMU 测量模型：

$$\left[\begin{array}{c}{\hat{\mathbf{\alpha }}}_{b_{k+1}}^{b_k}\\ {\hat{\mathbf{\beta }}}_{b_{k+1}}^{b_k}\\ {\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_w^{b_k}\left({\mathbf{p}}_{b_{k+1}}^w-{\mathbf{p}}_{b_k}^w+\frac{1}{2}{\mathbf{g}}^w\Delta t_k^2-{\mathbf{v}}_{b_k}^w\Delta t_k\right)\\ {\mathbf{R}}_w^{b_k}\left({\mathbf{v}}_{b_{k+1}}^w+{\mathbf{g}}^w\Delta t_k-{\mathbf{v}}_{b_k}^w\right)\\ {\mathbf{q}}_{b_k}^{w^{-1}}\otimes {\mathbf{q}}_{b_{k+1}}^w\\ {\mathbf{b}}_{a{b}_{k+1}}-{\mathbf{b}}_{a{b}_k}\\ {\mathbf{b}}_{w{b}_{k+1}}-{\mathbf{b}}_{w{b}_k}\end{array}\right]$$

----------------附：四元数求导----------------

四元数乘法：
设 $q_a=x_{a}i+y_{a}j+z_{a}k+s_a,q_b=x_{b}i+y_{b}j+z_{b}k+s_b$，令 ⊗ 表示四元数乘法运算符。
由虚部满足的关系式：
$$\begin{gathered} i^2=j^2=k^2=-1 \\ ij=k,ji=-k \\ jk=i,kj=-i \\ ki=j,ik=-j \end{gathered}$$
计算：
$$\begin{array}{rl}{q}_a\otimes q_b& =(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b)i\\ & +(s_a{y}_b+x_a{z}_b+y_a{s}_b-z_a{x}_b)j\\ & +(s_a{z}_b+x_a{y}_b+y_a{x}_b-z_a{s}_b)k\\ & +s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\end{array}$$
引入左乘和右乘符号，写成矩阵乘法形式：

令 $q=[xyzs{]}^T=[w^{T}s{]}^T$，有

其中 ^ 为反对称符号（见《十四讲》 P43）。

四元数的导数推导：

$\hat{k}\theta$ 是旋转向量。这里最后一行的 $\theta$ 是个三维向量。

感觉最后两行推导不太正确。
或许可以解释为旋转向量对时间求导为角速度？