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
注釋不完整，可以一起交流。