vins中imu融合_VINS-Mono代码分析与总结(一) IMU预积分

参考文献

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}

$$