VINS-Mono代码分析与总结(一) IMU预积分
Vins-Mono完整版总结：https://www.zybuluo.com/Xiaobuyi/note/866099
参考文献
1 VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, Tong Qin, Peiliang Li, Zhenfei Yang, Shaojie Shen (techincal report)
2 Solà J. Quaternion kinematics for the error-state KF[M]// Surface and Interface Analysis. 2015.
最近在读VINS-Mono的源码，IMU预积分部分的论文和代码不是很对应，自己根据代码和源码issues做了简单的的总结。
1 预积分的推导
__1.1 离散状态下预积分方程:__
关于这部分的论文和代码中的推导，可以参考文献[[2]](#[2])中Appendx部分“A Runge-Kutta numerical integration methods”中的欧拉法和中值法。 $$ w_{k}^{{}'}=\frac{w_{k+1}+w_{k}}{2}-b_{w} \tag{1.1} $$\[q _{i+1}=q _{i}\otimes \begin{bmatrix}
1
\\
0.5w_{k}^{{}'}\delta t
\end{bmatrix}
\tag{1.2}
\]
\[a_{k}^{{}'}=\frac{q_{k}(a_{k}+n_{a0}-b_{a_{k}})+q_{k+1}(a_{k+1}++n_{a1}-b_{a_{k}})}{2}
\tag{1.3}
\]
\[\alpha _{i+1}=\delta\alpha _{i}+\beta _{i}t+0.5a_{k}^{{}'}\delta t^{2}
\tag{1.4}
\]
\[\beta _{i+1}=\delta\beta _{i}+a_{k}^{{}'}\delta t
\tag{1.5}
\]
__1.2 离散状态下误差状态方程__
论文中Ⅱ.B部分的误差状态方程是连续时间域内，在实际代码中需要的是离散时间下的方程式，而且在前面的预积分方程中使用了中值法积分方式。所以在实际代码中和论文是不一致的。在推导误差状态方程式的最重要的部分是对 $\delta \theta _{k+1}$ 部分的推导。 由泰勒公式可得： $$ \delta \theta _{k+1} = \delta \theta _{k}+\dot{\delta \theta _{k}}\delta t \tag{2.1} $$ 依据参考文献[[2]](#[2])中 "5.3.3 The error-state kinematics"中公式(222c)及其推导过程有： $$ \dot{\delta \theta _{k}}=-[w_{m}-w_{b}]_{\times }\delta \theta _{k}-\delta w_{b}-w_{n} $$ 对于中值法积分下的误差状态方程为： $$ \dot{\delta \theta _{k}}=-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta \theta _{k}-\delta b_{g_{k}}+\frac{n_{w0}+n_{w1}}{2} \tag{2.2} $$ 将式(2.2)带入式(2.1)可得： $$ \delta \theta _{k+1} =(I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t) \delta \theta _{k} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t \tag{2.3} $$ 这部分也可以参考，文献[[2]](#[2])中“7.2 System kinematics in discrete time”小节。 接下来先推导 $\delta \beta _{k+1}$ 部分，再推导 $\delta \alpha _{k+1}$ 部分。$\delta \beta _{k+1}$ 部分的推导也可以参考文献[[2]](#[2])中“5.3.3 The error-state kinematics”公式(222b)的推导。将式(1.5)展开得到： $$ \delta\beta _{i+1}=\delta\beta _{i}+\frac{q_{k}(a_{k}+n_{a0}-b_{a_{k}})+q_{k+1}(a_{k+1}++n_{a1}-b_{a_{k}})}{2}\delta t $$ 即， $$ \delta\beta _{i+1}=\delta\beta _{i}+\dot{\delta\beta_{i}}\delta t \tag{2.4} $$ 文献[2]中，公式(222b) $$ \dot{\delta v}=-R[a_{m}-a_{b}]_{\times}\delta \theta-R\delta a_{b}+\delta g-Ra_{n} $$ 对于中值法积分下的误差状态方程为： $$ \begin{align}\notag \dot{\delta\beta_{i}} =&-\frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta \theta-\frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta \theta _{k+1} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t)\delta \theta \\\notag &-\frac{1}{2}q_{k}\delta b_{a_{k}}-\frac{1}{2}q_{k+1}\delta b_{a_{k}}-\frac{1}{2}q_{k}n_{a0}-\frac{1}{2}q_{k}n_{a1} \end{align} \tag{2.5} $$ 将式(2.3)带入式(2.5)可得 $$ \begin{align}\notag \dot{\delta\beta_{i}} =&-\frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta \theta-\frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}((I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t) \delta \theta _{k} -\delta b_{g_{k}}\delta t\\\notag &+\frac{n_{w0}+n_{w1}}{2}\delta t) -\frac{1}{2}q_{k}\delta b_{a_{k}}-\frac{1}{2}q_{k+1}\delta b_{a_{k}}-\frac{1}{2}q_{k}n_{a0}-\frac{1}{2}q_{k}n_{a1} \end{align} \tag{2.6} $$ 同理，可以计算出 $\delta \alpha _{k+1}$ ，可以写为： $$ \delta\alpha _{i+1}=\delta\alpha _{i}+\dot{\delta\alpha_{i}}\delta t \tag{2.7} $$ $$ \begin{align}\notag \dot{\delta\alpha_{i}} =&-\frac{1}{4}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta \theta\delta t-\frac{1}{4}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}((I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t) \delta \theta _{k} -\delta b_{g_{k}}\delta t \\\notag &+\frac{n_{w0}+n_{w1}}{2}\delta t)\delta t -\frac{1}{4}q_{k}\delta b_{a_{k}}\delta t-\frac{1}{4}q_{k+1}\delta b_{a_{k}}\delta t-\frac{1}{4}q_{k}n_{a0}\delta t-\frac{1}{4}q_{k}n_{a1}\delta t \end{align} \tag{2.8} $$ 最后是加速度计和陀螺仪bias的误差状态方程， $$ \delta b_{a_{k+1}}=\delta b_{a_{k}}+n_{ba}\delta t \tag{2.9} $$ $$ \delta b_{w_{k+1}}=\delta b_{w_{k}}+n_{bg}\delta t \tag{2.10} $$ 综合式(2.3)等误差状态方程，将其写为矩阵形式， $$ \begin{align}\notag \begin{bmatrix} \delta \alpha_{k+1}\\ \delta \theta _{k+1}\\ \delta \beta _{k+1} \\ \delta b _{a{}{k+1}} \\ \delta b _{g{}{k+1}} \end{bmatrix}&=\begin{bmatrix} I & f_{01} &\delta t & -\frac{1}{4}(q_{k}+q_{k+1})\delta t^{2} & f_{04}\\ 0 & I-[\frac{w_{k+1}+w_{k}}{2}-b_{wk}]_{\times }\delta t & 0 & 0&-\delta t \\ 0 & f_{21}&I & -\frac{1}{2}(q_{k}+q_{k+1})\delta t & f_{24}\\ 0 & 0& 0&I &0 \\ 0& 0 & 0 & 0 & I \end{bmatrix} \begin{bmatrix} \delta \alpha_{k}\\ \delta \theta _{k}\\ \delta \beta _{k} \\ \delta b _{a{}{k}} \\ \delta b _{g{}{k}} \end{bmatrix} \\\notag &+ \begin{bmatrix} \frac{1}{4}q_{k}\delta t^{2}& v_{01}& \frac{1}{4}q_{k+1}\delta t^{2} & v_{03} & 0 & 0\\ 0& \frac{1}{2}\delta t & 0 & \frac{1}{2}\delta t &0 & 0\\ \frac{1}{2}q_{k}\delta t& v_{21}& \frac{1}{2}q_{k+1}\delta t & v_{23} & 0 & 0 \\ 0 & 0 & 0 & 0 &\delta t &0 \\ 0& 0 &0 & 0 &0 & \delta t \end{bmatrix} \begin{bmatrix} n_{a0}\\ n_{w0}\\ n_{a1}\\ n_{w1}\\ n_{ba}\\ n_{bg} \end{bmatrix} \end{align} \tag{2.11} $$ 其中， $$ \begin{align}\notag f_{01}&=-\frac{1}{4}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta t^{2}-\frac{1}{4}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}(I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t)\delta t^{2} \\\notag f_{21}&=-\frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta t-\frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}(I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t)\delta t \\\notag f_{04}&=\frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})(-\delta t) \\\notag f_{24}&=\frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t)(-\delta t) \\\notag v_{01}&=\frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t \\\notag v_{03}&=\frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t \\\notag v_{21}&=\frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t \\\notag v_{23}&=\frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t \end{align} $$将式(2.11)简写为， $$ \delta z_{k+1} = F\delta z_{k}+VQ $$ 最后得到系统的雅克比矩阵 $J_{k+1}$ 和协方差矩阵 $P_{k+1}$，初始状态下的雅克比矩阵和协方差矩阵为单位阵和零矩阵，即 $$\notag J_{k}=I \\\notag P_{k}=0 $$ $$ J_{k+1}=FJ_{k} \tag{2.12} $$ $$ P_{k+1}=FP_{k}F^{T}+VQV_{T} \tag{2.13} $$VINS-Mono代码注释：https://github.com/gaochq/VINS-Mono/tree/comment
注释不完整，可以一起交流。
