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} ag{1.1} $$[q _{i+1}=q _{i}otimes egin{bmatrix}
1
\
0.5w_{k}^{{}'}delta t
end{bmatrix}
ag{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}
ag{1.3}
]
[alpha _{i+1}=deltaalpha _{i}+eta _{i}t+0.5a_{k}^{{}'}delta t^{2}
ag{1.4}
]
[eta _{i+1}=deltaeta _{i}+a_{k}^{{}'}delta t
ag{1.5}
]
__1.2 离散状态下误差状态方程__
论文中Ⅱ.B部分的误差状态方程是连续时间域内，在实际代码中需要的是离散时间下的方程式，而且在前面的预积分方程中使用了中值法积分方式。所以在实际代码中和论文是不一致的。在推导误差状态方程式的最重要的部分是对 $delta heta _{k+1}$ 部分的推导。 由泰勒公式可得： $$ delta heta _{k+1} = delta heta _{k}+dot{delta heta _{k}}delta t ag{2.1} $$ 依据参考文献[[2]](#[2])中 "5.3.3 The error-state kinematics"中公式(222c)及其推导过程有： $$ dot{delta heta _{k}}=-[w_{m}-w_{b}]_{ imes }delta heta _{k}-delta w_{b}-w_{n} $$ 对于中值法积分下的误差状态方程为： $$ dot{delta heta _{k}}=-[frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{ imes }delta heta _{k}-delta b_{g_{k}}+frac{n_{w0}+n_{w1}}{2} ag{2.2} $$ 将式(2.2)带入式(2.1)可得： $$ delta heta _{k+1} =(I-[frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{ imes }delta t) delta heta _{k} -delta b_{g_{k}}delta t+frac{n_{w0}+n_{w1}}{2}delta t ag{2.3} $$ 这部分也可以参考，文献[[2]](#[2])中“7.2 System kinematics in discrete time”小节。 接下来先推导 $delta eta _{k+1}$ 部分，再推导 $delta alpha _{k+1}$ 部分。$delta eta _{k+1}$ 部分的推导也可以参考文献[[2]](#[2])中“5.3.3 The error-state kinematics”公式(222b)的推导。将式(1.5)展开得到： $$ deltaeta _{i+1}=deltaeta _{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 $$ 即， $$ deltaeta _{i+1}=deltaeta _{i}+dot{deltaeta_{i}}delta t ag{2.4} $$ 文献[2]中，公式(222b) $$ dot{delta v}=-R[a_{m}-a_{b}]_{ imes}delta heta-Rdelta a_{b}+delta g-Ra_{n} $$ 对于中值法积分下的误差状态方程为： $$ egin{align} otag dot{deltaeta_{i}} =&-frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{ imes}delta heta-frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{ imes}delta heta _{k+1} -delta b_{g_{k}}delta t+frac{n_{w0}+n_{w1}}{2}delta t)delta heta \ otag &-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} ag{2.5} $$ 将式(2.3)带入式(2.5)可得 $$ egin{align} otag dot{deltaeta_{i}} =&-frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{ imes}delta heta-frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{ imes}((I-[frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{ imes }delta t) delta heta _{k} -delta b_{g_{k}}delta t\ otag &+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} ag{2.6} $$ 同理，可以计算出 $delta alpha _{k+1}$ ，可以写为： $$ deltaalpha _{i+1}=deltaalpha _{i}+dot{deltaalpha_{i}}delta t ag{2.7} $$ $$ egin{align} otag dot{deltaalpha_{i}} =&-frac{1}{4}q_{k}[a_{k}-b_{a_{k}}]_{ imes}delta hetadelta t-frac{1}{4}q_{k+1}[a_{k+1}-b_{a_{k}}]_{ imes}((I-[frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{ imes }delta t) delta heta _{k} -delta b_{g_{k}}delta t \ otag &+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} ag{2.8} $$ 最后是加速度计和陀螺仪bias的误差状态方程， $$ delta b_{a_{k+1}}=delta b_{a_{k}}+n_{ba}delta t ag{2.9} $$ $$ delta b_{w_{k+1}}=delta b_{w_{k}}+n_{bg}delta t ag{2.10} $$ 综合式(2.3)等误差状态方程，将其写为矩阵形式， $$ egin{align} otag egin{bmatrix} delta alpha_{k+1}\ delta heta _{k+1}\ delta eta _{k+1} \ delta b _{a{}{k+1}} \ delta b _{g{}{k+1}} end{bmatrix}&=egin{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}]_{ imes }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} egin{bmatrix} delta alpha_{k}\ delta heta _{k}\ delta eta _{k} \ delta b _{a{}{k}} \ delta b _{g{}{k}} end{bmatrix} \ otag &+ egin{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} egin{bmatrix} n_{a0}\ n_{w0}\ n_{a1}\ n_{w1}\ n_{ba}\ n_{bg} end{bmatrix} end{align} ag{2.11} $$ 其中， $$ egin{align} otag f_{01}&=-frac{1}{4}q_{k}[a_{k}-b_{a_{k}}]_{ imes}delta t^{2}-frac{1}{4}q_{k+1}[a_{k+1}-b_{a_{k}}]_{ imes}(I-[frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{ imes }delta t)delta t^{2} \ otag f_{21}&=-frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{ imes}delta t-frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{ imes}(I-[frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{ imes }delta t)delta t \ otag f_{04}&=frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{ imes}delta t^{2})(-delta t) \ otag f_{24}&=frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{ imes}delta t)(-delta t) \ otag v_{01}&=frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{ imes}delta t^{2})frac{1}{2}delta t \ otag v_{03}&=frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{ imes}delta t^{2})frac{1}{2}delta t \ otag v_{21}&=frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{ imes}delta t^{2})frac{1}{2}delta t \ otag v_{23}&=frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{ imes}delta t^{2})frac{1}{2}delta t end{align} $$将式(2.11)简写为， $$ delta z_{k+1} = Fdelta z_{k}+VQ $$ 最后得到系统的雅克比矩阵 $J_{k+1}$ 和协方差矩阵 $P_{k+1}$，初始状态下的雅克比矩阵和协方差矩阵为单位阵和零矩阵，即 $$ otag J_{k}=I \ otag P_{k}=0 $$ $$ J_{k+1}=FJ_{k} ag{2.12} $$ $$ P_{k+1}=FP_{k}F^{T}+VQV_{T} ag{2.13} $$VINS-Mono代码注释：https://github.com/gaochq/VINS-Mono/tree/comment
注释不完整，可以一起交流。