一步一步学习S-MSCKF连续时间IMU误差状态运动模型
Posted liuzhenbo
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了一步一步学习S-MSCKF连续时间IMU误差状态运动模型相关的知识，希望对你有一定的参考价值。
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}} ight]^{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 imes1})
({^{I}{dot p}(t)_{C}}=0_{3 imes1})
以上(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} imes]{^{G}v_{I}}+{[w_{G} imes]}^{2}{(^{G}p_{I})} ight)+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} ight)+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} ight]^{T})
数量关系：
(_{G}^{I}{dot{hat q}}=frac{1}{2}Omega(hat w)_{G}^{I}hat q)
(dot {hat b}_{g}(t)=0_{3 imes1})
(^{G}dot {hat v}_{I}=C(_{G}^{I}hat q)^{T}{hat a}+{^{G}g})
(dot {hat b}_{a}=0_{3 imes1})
(^{G}dot {hat p}_{I}={^{G}{hat v}_{I}})
({_{C}^{I}hat q}=0_{4 imes1})
({^{I}{hat p}_{C}}^{T}=0_{3 imes1})
其中：
(hat w = w_{m}-{hat b}_{g})
(hat a= a_{m}-{hat b}_{a})
(Omega(hat w)= left(egin{matrix} -[hat w_{ imes}] & {hat w} -{hat w}^{T} & 0 end{matrix} ight))
3 IMU误差状态运动模型
定义IMU误差状态：
将上述真实状态与估计状态做“差”：
( ilde x = x-{hat x})
其中四元数做差和普通的减法不一样，这里引入了误差四元数(delta q)来表示旋转误差：
(q={delta q} igotimes {hat q})
({delta q}simeq [frac{1}{2}{delta heta}^{T},1]^{T})
所以可以用三维向量(delta heta)来表示旋转误差，从而定义IMU误差状态向量为：
( ilde x_{I} =left[{{_{G}^{I}{ ilde heta}^{T}},{ ilde b_{g}}^{T},{^{G} ilde v_{I}}^{T},{ ilde b_{a}}^{T},{^{G} ilde p_{I}}^{T}},{_{C}^{I}{ ilde heta}^{T}},{^{I}{ ilde p}_{C}}^{T} ight]^{T})
连续误差状态运动方程：
({dot { ilde x}}_{I}=F{ ilde x_{I}}+G{n}_{I})
其中(n_{I}^{T}=left({n_{g}^{T}},{n_{wg}^{T}},{n_{a}^{T}},{n_{wa}^{T}} ight)^{T})。向量(n_{g})和(n_{a})代表陀螺仪与加速度计的测量噪声(高斯)，(n_{wg})和(n_{wa})是陀螺仪与加速度计biases的随机游走速率。
4 推导F与G
4.1 求(delta heta)
对(_{G}^{I}q=delta q igotimes{_{I}^{G}hat q})左右两边同时求导得到：
(_{G}^{I}dot q=dot {delta q} igotimes {_{G}^{I}hat q} + delta q igotimes {_{G}^{I}dot {hat q}})
(frac{1}{2}left[egin{matrix} w end{matrix} ight] igotimes {_{G}^{I}q}= dot {delta q} igotimes {_{G}^{I}{hat q}}+ delta q igotimes frac{1}{2}left[egin{matrix} {hat w} end{matrix} ight] igotimes {_{G}^{I}{hat q}})
两边同时乘以({_{G}^{I}{hat q}}^{-1})得到：
(frac{1}{2}left[egin{matrix} w end{matrix} ight] igotimes {delta q}= dot {delta q}+ frac{1}{2}delta q igotimes left[egin{matrix} {hat w} end{matrix} ight])
整理得：
(left[egin{matrix} {dot {delta heta}} {dot {2}} end{matrix} ight]= left[egin{matrix} w end{matrix} ight] igotimes delta q- delta q igotimesleft[egin{matrix} {hat w} end{matrix} ight])
由(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 - { ilde b}_{g}-n_{g})
带入(w)得到：
(left[egin{matrix} {dot {delta heta}} {0} end{matrix} ight]= left[egin{matrix} {hat w - { ilde b}_{g}-n_{g}} end{matrix} ight] igotimes delta q- delta q igotimesleft[egin{matrix} {hat w} end{matrix} ight])
(left[egin{matrix} {dot {delta heta}} {0} end{matrix} ight]= left[egin{matrix} {hat w} end{matrix} ight] igotimes delta q- delta q igotimesleft[egin{matrix} {hat w} end{matrix} ight]- left[egin{matrix} {{ ilde b}_{g}+n_{g}} end{matrix} ight] igotimes delta q)
利用四元数乘法的性质得到：
(left[egin{matrix} {dot {delta heta}} {0} end{matrix} ight]= left[egin{matrix} -[{hat w}_{ imes}] & {hat w} -{hat w}^{T} & 0 end{matrix} ight] delta q- left[egin{matrix} [{hat w}_{ imes}] & {hat w} -{hat w}^{T} & 0 end{matrix} ight]delta q- left[egin{matrix} -[{( ilde b_{g}+n_{g})}_{ imes}] & {( ilde b_{g}+n_{g})} -{( ilde b_{g}+n_{g})}^{T} & 0 end{matrix} ight] left[egin{matrix} {frac{1}{2}delta heta} {1} end{matrix} ight])
忽略掉极小量相乘的项得：
(left[egin{matrix} {dot {delta heta}} {0} end{matrix} ight]= left[egin{matrix} -2[{hat w}_{ imes}] & 0 & 0 end{matrix} ight] left[egin{matrix} {frac{1}{2}delta heta} {1} end{matrix} ight]- left[egin{matrix} { ilde b_{g}+n_{g}} {0} end{matrix} ight])
({dot {delta heta}}=-[{hat w}_{ imes}]delta heta - ilde b_{g} -n_{g})
4.2 求(dot{ ilde b}_{g})
(dot{ ilde b}_{g}=dot b_{g}-dot{hat b}_{g}=n_{wg})
4.3 求(^{G}dot { ilde v}_{I})
(^{G}dot { ilde 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} ight)+{^{G}g})
(^{G}dot {hat v}_{I}=C(_{G}^{I}hat q)^{T}{hat a}+{^{G}g})
(^{G}dot { ilde v}_{I}={C(_{G}^{I}q)}^{T}left(a_{m}-b_{a}-n_{a} ight)+{^{G}g}-C(_{G}^{I}hat q)^{T}{hat a}-{^{G}g})
(={C(_{G}^{I}{hat q})}^{T}left(I+[delta heta_{ imes}] ight)left(a_{m}-b_{a}-n_{a} ight)-C(_{G}^{I}hat q)^{T}{hat a})
由(hat a=a_{m}-hat b_{a})得：
(={C(_{G}^{I}{hat q})}^{T}left(I+[delta heta_{ imes}] ight)left(hat a- ilde b_{a}-n_{a} ight)-C(_{G}^{I}hat q)^{T}{hat a})
省略掉高次项([delta heta_{ imes}]left(- ilde b_{a}-n_{a} ight))得到：
(={C(_{G}^{I}{hat q})}^{T}left([delta heta_{ imes}]hat a - ilde b_{a}-n_{a} ight))
(=-{C(_{G}^{I}{hat q})}^{T}[hat a_{ imes}]delta heta-{C(_{G}^{I}{hat q})}^{T} ilde b_{a}-{C(_{G}^{I}{hat q})}^{T}n_{a})
4.4 求(dot { ilde b}_{a})
(dot { ilde b}_{a}=dot b_{a}-dot {hat b}_{a}=n_{wa})
4.5 求(^{G}dot { ilde p}_{I})
(^{G}dot { ilde p}_{I}={^{G}p_{I}}-{^{G}dot {hat p}_{I}})
(={^{G}v_{I}}-{^{G}{hat v}_{I}}={^{G}{ ilde v}_{I}})
4.6求(_{C}^{I} ilde heta)和(^{I} ilde p_{C})
(_{C}^{I} ilde heta)和(^{I} ilde p_{C})是相机与IMU的相对位置，所以有：
(_{C}^{I} ilde heta=0_{3 imes 1})
(^{I} ilde p_{C}=0_{3 imes 1})
4.7 写成矩阵形式
参考资料
(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
以上是关于一步一步学习S-MSCKF连续时间IMU误差状态运动模型的主要内容，如果未能解决你的问题，请参考以下文章