一步一步推导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\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\times 1}$

${}^I\dot{p}(t{)}_C=0_{3\times 1}$

以上$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\times 1}$

${}^G{\dot{\hat{v}}}_I=C{(}_G^I\hat{q}{)}^T\hat{a}+{}^{G}g$

${\dot{\hat{b}}}_a=0_{3\times 1}$

${}^G{\dot{\hat{p}}}_I={}^G{\hat{v}}_I$

${}_C^I\hat{q}=0_{4\times 1}$

${{}^I{\hat{p}}_C}^T=0_{3\times 1}$

其中：

$\hat{w}=w_m-{\hat{b}}_g$

$\hat{a}=a_m-{\hat{b}}_a$

$\Omega (\hat{w})=\left(\begin{array}{cc}-[{\hat{w}}_{\times }]& \hat{w}\\ -{\hat{w}}^T& 0\end{array}\right)$

3 IMU误差状态运动模型

定义IMU误差状态：

将上述真实状态与估计状态做“差”：

$\tilde{x}=x-\hat{x}$

其中四元数做差和普通的减法不一样，这里引入了误差四元数$\delta q$来表示旋转误差：

$q=\delta q⨂\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⨂{}_I^G\hat{q}$左右两边同时求导得到：

${}_G^I\dot{q}=\dot{\delta q}⨂{}_G^I\hat{q}+\delta q⨂{}_G^I\dot{\hat{q}}$

$\frac{1}{2}\left[\begin{array}{c}w\\ 0\end{array}\right]⨂{}_G^{I}q=\dot{\delta q}⨂{}_G^I\hat{q}+\delta q⨂\frac{1}{2}\left[\begin{array}{c}\hat{w}\\ 0\end{array}\right]⨂{}_G^I\hat{q}$

两边同时乘以${{}_G^I\hat{q}}^{-1}$得到：

$\frac{1}{2}\left[\begin{array}{c}w\\ 0\end{array}\right]⨂\delta q=\dot{\delta q}+\frac{1}{2}\delta q⨂\left[\begin{array}{c}\hat{w}\\ 0\end{array}\right]$

整理得：

$\left[\begin{array}{c}\dot{\delta \theta }\\ \dot{2}\end{array}\right]=\left[\begin{array}{c}w\\ 0\end{array}\right]⨂\delta q-\delta q⨂\left[\begin{array}{c}\hat{w}\\ 0\end{array}\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{array}{c}\dot{\delta \theta }\\ 0\end{array}\right]=\left[\begin{array}{c}\hat{w}-{\tilde{b}}_g-n_g\\ 0\end{array}\right]⨂\delta q-\delta q⨂\left[\begin{array}{c}\hat{w}\\ 0\end{array}\right]$

$\left[\begin{array}{c}\dot{\delta \theta }\\ 0\end{array}\right]=\left[\begin{array}{c}\hat{w}\\ 0\end{array}\right]⨂\delta q-\delta q⨂\left[\begin{array}{c}\hat{w}\\ 0\end{array}\right]-\left[\begin{array}{c}{\tilde{b}}_g+n_g\\ 0\end{array}\right]⨂\delta q$

利用四元数乘法的性质得到：

$\left[\begin{array}{c}\dot{\delta \theta }\\ 0\end{array}\right]=\left[\begin{array}{cc}-[{\hat{w}}_{\times }]& \hat{w}\\ -{\hat{w}}^T& 0\end{array}\right]\delta q-\left[\begin{array}{cc}[{\hat{w}}_{\times }]& \hat{w}\\ -{\hat{w}}^T& 0\end{array}\right]\delta q-\left[\begin{array}{cc}-[{({\tilde{b}}_g+n_g)}_{\times }]& ({\tilde{b}}_g+n_g)\\ -{({\tilde{b}}_g+n_g)}^T& 0\end{array}\right]\left[\begin{array}{c}\frac{1}{2}\delta \theta \\ 1\end{array}\right]$

忽略掉极小量相乘的项得：

$\left[\begin{array}{c}\dot{\delta \theta }\\ 0\end{array}\right]=\left[\begin{array}{cc}-2[{\hat{w}}_{\times }]& 0\\ 0& 0\end{array}\right]\left[\begin{array}{c}\frac{1}{2}\delta \theta \\ 1\end{array}\right]-\left[\begin{array}{c}{\tilde{b}}_g+n_g\\ 0\end{array}\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{array}{ccccccc}-[{\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({}_G^I\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{array}\right]{\tilde{x}}_I+\left[\begin{array}{cccc}-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({}_G^I\hat{q}\right)}^T& 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}\end{array}\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