亲宝软件园·资讯

展开

一步一步学习S-MSCKF(一)连续时间IMU误差状态运动模型

心田居士 人气:1
## 1 IMU真实状态运动模型 **状态向量:** $x_{I}=\left[{{_{G}^{I}{q(t)}}^{T},{b_{g}(t)}^{T},{^{G}v_{I}(t)}^{T},{b_{a}(t)}^{T},{^{G}p_{I}(t)}^{T}},{{_{C}^{I}q(t)}^{T}},{{^{I}p(t)_{C}}^{T}}\right]^{T}$ 四元数${_{G}^{I}{q(t)}}$代表惯性系到IMU坐标系的旋转,${^{G}v_{I}(t)}$和${^{G}p_{I}(t)}$代表IMU坐标系在惯性系中的速度和位置,${b_{g}(t)}$和${b_{a}(t)}$表示在IMU坐标系中测量值角速度与线加速度的biases,${_{C}^{I}q(t)}$和${^{I}p(t)_{C}}$表示相机坐标系和IMU坐标系的相对位置,其中相机坐标系取左相机坐标系。 **数量关系:** $_{G}^{I}{\dot q(t)}=\frac{1}{2}\Omega(w(t))_{G}^{I}q(t)$ $\dot b_{g}(t)=n_{wg}(t)$ $^{G}\dot v_{I}(t)={^{G}a_{I}(t)}$ $\dot b_{a}(t)=n_{wa}(t)$ $^{G}\dot p_{I}(t)={^{G}v_{I}(t)}$ ${_{C}^{I}{\dot q(t)}}=0_{4\times1}$ ${^{I}{\dot p}(t)_{C}}=0_{3\times1}$ 以上$w(t)=[w_{x}(t),w_{y}(t),w_{z}(t)]^{T}$是IMU角速度在IMU系中的坐标。 **IMU的观测值为:** $w_{m}=w+C(_{G}^{I}q)w_{G}+b_{g}+n_{g}$ $a_{m}=C(_{G}^{I}q)\left(^{G}a_{I}-{^{G}g}+2[w_{G}\times]{^{G}v_{I}}+{[w_{G}\times]}^{2}{(^{G}p_{I})}\right)+b_{a}+n_{a}$ 其中$w_{G}$为地球的自转速度在G系的坐标,在某些VIO实现中,会将地球自转的影响忽略不计,比如S-MSCKF,以后的推导中也会不计地球自转影响。 **将地球自转忽略:** $w_{m}=w+b_{g}+n_{g}$ $a_{m}=C(_{G}^{I}q)\left(^{G}a_{I}-{^{G}g}\right)+b_{a}+n_{a}$ ## 2 IMU估计状态运动模型 **状态向量:** $\hat x_{I} =\left[{{_{G}^{I}{\hat q}}^{T},{\hat b_{g}}^{T},{^{G}\hat v_{I}}^{T},{\hat b_{a}}^{T},{^{G}\hat p_{I}}^{T}},{_{C}^{I}\hat q}^{T},{^{I}{\hat p}_{C}}^{T}\right]^{T}$ **数量关系:** $_{G}^{I}{\dot{\hat q}}=\frac{1}{2}\Omega(\hat w)_{G}^{I}\hat q$ $\dot {\hat b}_{g}(t)=0_{3\times1}$ $^{G}\dot {\hat v}_{I}=C(_{G}^{I}\hat q)^{T}{\hat a}+{^{G}g}$ $\dot {\hat b}_{a}=0_{3\times1}$ $^{G}\dot {\hat p}_{I}={^{G}{\hat v}_{I}}$ ${_{C}^{I}\hat q}=0_{4\times1}$ ${^{I}{\hat p}_{C}}^{T}=0_{3\times1}$ 其中: $\hat w = w_{m}-{\hat b}_{g}$ $\hat a= a_{m}-{\hat b}_{a}$ $\Omega(\hat w)= \left(\begin{matrix} -[\hat w_{\times}] & {\hat w} \\ -{\hat w}^{T} & 0 \end{matrix}\right)$ ## 3 IMU误差状态运动模型 **定义IMU误差状态:** 将上述真实状态与估计状态做“差”: $\tilde x = x-{\hat x}$ 其中四元数做差和普通的减法不一样,这里引入了误差四元数$\delta q$来表示旋转误差: $q={\delta q} \bigotimes {\hat q}$ ${\delta q}\simeq [\frac{1}{2}{\delta\theta}^{T},1]^{T}$ 所以可以用三维向量$\delta\theta$来表示旋转误差,从而定义IMU误差状态向量为: $\tilde x_{I} =\left[{{_{G}^{I}{\tilde\theta}^{T}},{\tilde b_{g}}^{T},{^{G}\tilde v_{I}}^{T},{\tilde b_{a}}^{T},{^{G}\tilde p_{I}}^{T}},{_{C}^{I}{\tilde\theta}^{T}},{^{I}{\tilde p}_{C}}^{T}\right]^{T}$ **连续误差状态运动方程:** ${\dot {\tilde x}}_{I}=F{\tilde x_{I}}+G{n}_{I}$ 其中$n_{I}^{T}=\left({n_{g}^{T}},{n_{wg}^{T}},{n_{a}^{T}},{n_{wa}^{T}}\right)^{T}$。向量$n_{g}$和$n_{a}$代表陀螺仪与加速度计的测量噪声(高斯),$n_{wg}$和$n_{wa}$是陀螺仪与加速度计biases的随机游走速率。 ## 4 推导F与G ### 4.1 求$\delta\theta$ 对$_{G}^{I}q=\delta q \bigotimes{_{I}^{G}\hat q}$左右两边同时求导得到: $_{G}^{I}\dot q=\dot {\delta q} \bigotimes {_{G}^{I}\hat q} + \delta q \bigotimes {_{G}^{I}\dot {\hat q}}$ $\frac{1}{2}\left[\begin{matrix} w \\ 0 \end{matrix}\right] \bigotimes {_{G}^{I}q}= \dot {\delta q} \bigotimes {_{G}^{I}{\hat q}}+ \delta q \bigotimes \frac{1}{2}\left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right] \bigotimes {_{G}^{I}{\hat q}}$ 两边同时乘以${_{G}^{I}{\hat q}}^{-1}$得到: $\frac{1}{2}\left[\begin{matrix} w \\ 0 \end{matrix}\right] \bigotimes {\delta q}= \dot {\delta q}+ \frac{1}{2}\delta q \bigotimes \left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right]$ 整理得: $\left[\begin{matrix} {\dot {\delta\theta}} \\ {\dot {2}} \end{matrix}\right]= \left[\begin{matrix} w \\ 0 \end{matrix}\right] \bigotimes \delta q- \delta q \bigotimes\left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right]$ 由$w_{m}=w+b_{g}+n_{g}$和$\hat w =w_{m}-{\hat b}_{g}$可以求出$w$: $w=\hat w + {\hat b}_{g}-b_{g}-n_{g}=\hat w - {\tilde b}_{g}-n_{g}$ 带入$w$得到: $\left[\begin{matrix} {\dot {\delta\theta}} \\ {0} \end{matrix}\right]= \left[\begin{matrix} {\hat w - {\tilde b}_{g}-n_{g}} \\ 0 \end{matrix}\right] \bigotimes \delta q- \delta q \bigotimes\left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right]$ $\left[\begin{matrix} {\dot {\delta\theta}} \\ {0} \end{matrix}\right]= \left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right] \bigotimes \delta q- \delta q \bigotimes\left[\begin{matrix} {\hat w} \\ 0 \end{matrix}\right]- \left[\begin{matrix} {{\tilde b}_{g}+n_{g}} \\ 0 \end{matrix}\right] \bigotimes \delta q$ 利用四元数乘法的性质得到: $\left[\begin{matrix} {\dot {\delta\theta}} \\ {0} \end{matrix}\right]= \left[\begin{matrix} -[{\hat w}_{\times}] & {\hat w} \\ -{\hat w}^{T} & 0 \end{matrix}\right] \delta q- \left[\begin{matrix} [{\hat w}_{\times}] & {\hat w} \\ -{\hat w}^{T} & 0 \end{matrix}\right]\delta q- \left[\begin{matrix} -[{(\tilde b_{g}+n_{g})}_{\times}] & {(\tilde b_{g}+n_{g})} \\ -{(\tilde b_{g}+n_{g})}^{T} & 0 \end{matrix}\right] \left[\begin{matrix} {\frac{1}{2}\delta\theta} \\ {1} \end{matrix}\right]$ 忽略掉极小量相乘的项得: $\left[\begin{matrix} {\dot {\delta\theta}} \\ {0} \end{matrix}\right]= \left[\begin{matrix} -2[{\hat w}_{\times}] & 0 \\ 0 & 0 \end{matrix}\right] \left[\begin{matrix} {\frac{1}{2}\delta\theta} \\ {1} \end{matrix}\right]- \left[\begin{matrix} {\tilde b_{g}+n_{g}} \\ {0} \end{matrix}\right]$ ${\dot {\delta\theta}}=-[{\hat w}_{\times}]\delta\theta -\tilde b_{g} -n_{g}$ ### 4.2 求$\dot{\tilde b}_{g}$ $\dot{\tilde b}_{g}=\dot b_{g}-\dot{\hat b}_{g}=n_{wg}$ ### 4.3 求$^{G}\dot {\tilde v}_{I}$ $^{G}\dot {\tilde v}_{I}={^{G}\dot v_{I}-^{G}\dot {\hat v}_{I}}$ $^{G}\dot v_{I}={^{G}a_{I}}={C(_{G}^{I}q)}^{T}\left(a_{m}-b_{a}-n_{a}\right)+{^{G}g}$ $^{G}\dot {\hat v}_{I}=C(_{G}^{I}\hat q)^{T}{\hat a}+{^{G}g}$ $^{G}\dot {\tilde v}_{I}={C(_{G}^{I}q)}^{T}\left(a_{m}-b_{a}-n_{a}\right)+{^{G}g}-C(_{G}^{I}\hat q)^{T}{\hat a}-{^{G}g}$ $={C(_{G}^{I}{\hat q})}^{T}\left(I+[\delta\theta_{\times}]\right)\left(a_{m}-b_{a}-n_{a}\right)-C(_{G}^{I}\hat q)^{T}{\hat a}$ 由$\hat a=a_{m}-\hat b_{a}$得: $={C(_{G}^{I}{\hat q})}^{T}\left(I+[\delta\theta_{\times}]\right)\left(\hat a-\tilde b_{a}-n_{a}\right)-C(_{G}^{I}\hat q)^{T}{\hat a}$ 省略掉高次项$[\delta\theta_{\times}]\left(-\tilde b_{a}-n_{a}\right)$得到: $={C(_{G}^{I}{\hat q})}^{T}\left([\delta\theta_{\times}]\hat a -\tilde b_{a}-n_{a}\right)$ $=-{C(_{G}^{I}{\hat q})}^{T}[\hat a_{\times}]\delta\theta-{C(_{G}^{I}{\hat q})}^{T}\tilde b_{a}-{C(_{G}^{I}{\hat q})}^{T}n_{a}$ ### 4.4 求$\dot {\tilde b}_{a}$ $\dot {\tilde b}_{a}=\dot b_{a}-\dot {\hat b}_{a}=n_{wa}$ ### 4.5 求$^{G}\dot {\tilde p}_{I}$ $^{G}\dot {\tilde p}_{I}={^{G}p_{I}}-{^{G}\dot {\hat p}_{I}}$ $={^{G}v_{I}}-{^{G}{\hat v}_{I}}={^{G}{\tilde v}_{I}}$ ### 4.6求$_{C}^{I}\tilde \theta$和$^{I} \tilde p_{C}$ $_{C}^{I}\tilde \theta$和$^{I} \tilde p_{C}$是相机与IMU的相对位置,所以有: $_{C}^{I}\tilde \theta=0_{3\times 1}$ $^{I} \tilde p_{C}=0_{3\times 1}$ ### 4.7 写成矩阵形式 $$\dot {\tilde x}_{I}=\left[\begin{matrix} -[\hat w_{\times}] & -I_{3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3}\\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ -C\left({_{G}^{I}\hat q}\right)^{T}[\hat a_{\times}] & 0_{3 \times 3} & 0_{3 \times 3} & -C\left(^{I}_{G}\hat q\right)^{T} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & I_{3\times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \end{matrix}\right]{\tilde x_{I}}+ \left[\begin{matrix} -I_{3\times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3\times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & -C\left(^{I}_{G}\hat q\right)^{T} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & I_{3\times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \end{matrix}\right]{n_{I}}$$ ## 参考资料 (1) A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation (2) Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight (3) Indirect Kalman Filter for 3D Attitude Estimation

加载全部内容

相关教程
猜你喜欢
用户评论