美文网首页
The error state in MSCKF

The error state in MSCKF

作者: 轻骑兵1390 | 来源:发表于2020-07-16 13:50 被阅读0次

1. IMU state vector

{x}_{I}= \begin{bmatrix} ^I_G{q} & ^G{p} & ^G{V} & {b}_{g} & {b}_{a} \end{bmatrix}^{T}

  • All elements are column vectors. We ignore their transpose symbol.
  • ^I_G{q} is the quaternion from the global frame to the IMU frame.
  • ^G{p} is the pose with respect to global frame.
  • ^G{V} is the velocity with respect to global frame.
  • b_g and b_a are biases of gyroscope and accelerometer, respectively.
  • \color{red}{Attention}: MSCKF uses JPL quaternion, which is left-hand. Some operations is different from the right-hand quaternion, Hamilton quaternion.

2. The continuous-time motion dynamics of the IMU

^I_G\dot{q}^{T} = \frac{1}{2}\Omega(^I{w}(t)) \cdot \space ^I_G{q}(t) \tag{1}

^G{\dot{p}(t)} = \space^G{v}(t) \tag{2}

^G{\dot{v}(t)} = \space^G{a}(t) \tag{3}

\dot{b}_g(t) = n_{wg}(t) \tag{4}

\dot{b}_a(t) = n_{wa}(t) \tag{5}

  • The equation(1) resembles Possion equation where
    \Omega(w)= \begin{bmatrix} -[w_\times] & w \\ w^T & 0 \end{bmatrix}
  • The relationship between equation (1) and Possion's equation is noted in the ordinary differential equation about rotation.
  • (2) and (3) represents the derivatives of pose is velocity and that of velocity is acceleration, where all variable are respected to globel frame.
  • (4) and (5) means the derivative of bias is gaussian noise.

3. Error state

\tilde{x}_{I}= \begin{bmatrix} ^I\tilde{\theta} & ^G\tilde{p} & ^G\tilde{V} & \tilde{b}_{g} & \tilde{b}_{a} \end{bmatrix}^{T}

  • The error represent the different between true value and estimated value, such as ^G\tilde{p} = \space^G{p}-\space^G\hat{p};
  • The orientation error ^I\tilde{\theta} with respect to \color{red} {body} frame satisifies the following equation:
    ^I_G{R} \approx(I_3-[^I\tilde{\theta}_\times]) \cdot \space^I_G\hat{R} \tag{6}
    where (I-\tilde\theta) is because of JPL quaternions.

4. Error Propagation

4.1 Orientation

From time step l to l+1, the estimated rotation matrix is
\hat{R}_{l+1|l} = \space^{I_{l+1}}_{I_{l}}\hat{R} \cdot \hat{R}_{l|l} \tag{7}
where

  • \hat{R}_{l|l} = R(^I_G\hat{q}_{l|l}).
  • I_l is the IMU frame at time l.

We define the error state \theta by

^{I_{l+1}}_{I_l}{R} \approx(I_3-[\tilde{\theta}_\times]) \cdot \space^{I_{l+1}}_{I_l}\hat{R} \tag{8}
where ^{I_{l+1}}_{G}{R} = \space^{I_{l+1}}_{I_l}{R} \cdot \space^{I_{l}}_{G}{R},
From the above functions, (7) and (8), we obtain the following expression for the linearized error propagation:
\begin{aligned} ^{I} \tilde{\boldsymbol{\theta}}_{\ell+1 \mid \ell} &\simeq \space^{I_{\ell+1}}_{I_{\ell}} \hat{\mathbf{R}} \cdot^{I} \tilde{\boldsymbol{\theta}}_{\ell \mid \ell}+\tilde{\boldsymbol{\theta}}_{\Delta \ell}\\ &\simeq \hat{\mathbf{R}}_{\ell+1 \mid \ell} \hat{\mathbf{R}}_{\ell \mid \ell}^{T} \cdot^{I} \tilde{\boldsymbol{\theta}}_{\ell \mid \ell}+\tilde{\boldsymbol{\theta}}_{\Delta \ell} \end{aligned} \tag{9}
The function is represented by equation (15) in msckf2.0 and its detail is shown in The Detail of The Error State in MSCKF.

4.2 Velocity

The velocity propation equation is
^G\mathbf{v}_{l+1|l} = ^G\mathbf{v}_{l|l} + \int_{t_l}^{t_{l+1}}{\space^G\mathbf{a}_\tau d\tau} \tag{10}
where \tau represent the integral of time and ^G\mathbf{a} is the acceleration with respect to global frame.
^G\mathbf{v}_{l+1|l} = ^G\mathbf{v}_{l|l} + \int_{t_l}^{t_{l+1}}{\left( \space^G_{I_\tau} \mathbf{R}\cdot \space^{I_\tau}\mathbf{a}_m + \space^G\mathbf{g} \right) d\tau} \tag{11}
where \space^{I_\tau}\mathbf{a}_m is imu measurement in body frame at time \tau. \space^G_{I_\tau}\mathbf{R}\cdot \space^{I_\tau}\mathbf{a}_m compute the measurement with repsect to global frame. \space^G_{I_\tau} \mathbf{R}\cdot \space^{I_\tau}\mathbf{a}_m + \space^G\mathbf{g} add gravity, where the imu measurement contains a negative gravity (-\mathbf{g}) as following:
\mathbf{a}_m = \space^I_G\mathbf{R}(\space^G\mathbf{a}-\space^G\mathbf{g})+\mathbf{b}_\mathbf{a}+\mathbf{n}_\mathbf{a} \tag{12}
where \mathbf{b}_\mathbf{a} is bias and \mathbf{n}_\mathbf{a} is noise.

We define s_l = \int_{t_l}^{t_{l+1}}\space_{I_\tau}^{I_l}\mathbf{R} \cdot \space^{I_\tau}\mathbf{a}_m d\tau and rebuild the velocity propagation as following:
^G\mathbf{v}_{l+1|l} = ^G\mathbf{v}_{l|l} + \space^G\mathbf{g} \Delta t + \mathbf{R}^T_{l|l}\mathbf{s}_l \tag{13}

Finally the error state of velocity is represented as following:
^G \tilde{\mathbf{v}}_{l+1|l} \simeq -\hat{\mathbf{R}}_{l|l}^{T} \cdot [\hat{s}_l]_\times \space^I\tilde{\theta}_{l|l} + \space^G\tilde{\mathbf{v}}_{l|l} + \hat{\mathbf{R}}^T_{l|l}\tilde{s}_l \tag{14}
where \tilde s only depends on the IMU noise. The detail of function is shown in The Detail of The Error State in MSCKF.

4.3 Pose
The position of IMU propagation is:
\begin{aligned} ^G \mathbf{p}_{l+1|l} &= \space^G \mathbf{p}_{l|l} + \int_{t_l}^{t_{l+1}}\space^G \mathbf{v}_\tau d\tau \\ &= \space^G \mathbf{p}_{l|l} + \space^G \mathbf{v}_{l|l} \cdot \Delta t + \frac{1}{2}\space^G g \Delta t^2 + \mathbf{R}_{l|l}^T \mathbf{y}_l \end{aligned} \tag{15}
where \mathbf{y}_l =\int_{t_{l}}^{t_{l}+1} \int_{t_{l}}^{s} I_{\tau} \hat{\mathbf{R}}\space^{I_{\tau}} \mathbf{a}_{m} d \tau d s. And proceeding to error-state model, we obtain:
^G\tilde{p}_{l+1|l} \simeq -\hat{\mathbf{R}}_{l|l}^T \cdot [\hat{\mathbf{y}}_l]_\times \space^I\tilde{\theta}_{l|l}+\space^G\mathbf{\tilde{v}}_{l|l}\Delta t + \space^G\tilde{\mathbf{p}}_{l|l} + \tilde{\mathbf{R}}_{l|l}^T\tilde{\mathbf{y}}_l \tag{16}
By combining (9), (14) and (16), the error propagation function in MSCKF is writen as following:
\underbrace{\begin{bmatrix} ^I \tilde{\boldsymbol{\theta}}_{l+1|l} \\ ^G {\tilde{\mathbf{p}}_{l+1|l}} \\ ^G {\tilde{\mathbf{V}}_{l+1|l}} \end{bmatrix}}_{\tilde{\mathbf{x}}_{I_{l+1|l}}}= \underbrace{\begin{bmatrix} \hat{\mathbf{R}}_{l+1|l} \hat{\mathbf{R}}_{l|l}^{T} & \mathbf{0}_{3} & \mathbf{0}_{3} \\ -\hat{\mathbf{R}}_{l|l}^{T} [\hat{\mathbf{y}}_{l}]_\times & \mathbf{I}_{3} & \Delta t \mathbf{I}_{3} \\ -\hat{\mathbf{R}}_{l|l}^{T} [\hat{\mathbf{s}}_{l}]_\times & \mathbf{0}_{3} & \mathbf{I}_{3} \end{bmatrix}}_{\mathbf{\Phi}_{I_{l}}} \underbrace{\begin{bmatrix} ^I \tilde{\boldsymbol{\theta}}_{l|l} \\ ^G{\tilde{\mathbf{p}}}_{l|l} \\ ^G{\tilde{\mathbf{v}}}_{l|l} \end{bmatrix}}_{\tilde{\mathbf{x}}_{I_{\ell \mid \ell}}}+ \underbrace{\begin{bmatrix} \tilde{\boldsymbol{\theta}}_{\Delta \ell} \\ \hat{\mathbf{R}}_{l|l}^{T} \tilde{\mathbf{y}}_l \\ \hat{\mathbf{R}}_{l|l}^{T} \tilde{\mathbf{s}}_{l} \end{bmatrix}}_{\mathbf{w}_{l}} \tag{17}
\hat{\mathbf{s}}_l and \hat{\mathbf{y}}_l can be writen by:
\begin{aligned} \begin{array}{l} \hat{\mathbf{s}}_{l}=\hat{\mathbf{R}}_{l|l}({ }^{G} \hat{\mathbf{v}}_{l+1|l}-{ }^{G} \hat{\mathbf{v}}_{l|l}-{ }^{G} {g} \Delta t) \\ \hat{\mathbf{y}}_l=\hat{\mathbf{R}}_{l|l}({ }^{G} \hat{\mathbf{p}}_{l+1|l}-{ }^{G} \hat{\mathbf{p}}_{l|l}-{ }^{G} \hat{\mathbf{v}}_{l|l} \Delta t-\frac{1}{2} \space^G g\Delta t^{2}) \end{array} \end{aligned}

相关文章

网友评论

      本文标题：The error state in MSCKF

      本文链接：https://www.haomeiwen.com/subject/ymephktx.html

      延伸阅读

      深度阅读

      • 您也可以注册成为美文阅读网的作者，发表您的原创作品、分享您的心情！

      栏目导航

      热点阅读

      关于我们|服务条款|联系我们|The error state in MSCKF|投稿指南|网站地图|RSS订阅|排版工具|手机版

      提供经典美文摘抄,优美散文欣赏,现代诗歌精选,短篇小说,心情随笔,表白情书范文,故事会在线阅读欣赏

      Copyright © 2014-2023 Haomeiwen.com All Rights Reserved. 好美文阅读网 版权所有

      备案信息：桂公网安备 45052102000051号 · 桂ICP备13007215号-3

      本站所收录作品、热点评论等信息部分来源互联网，目的只是为了系统归纳学习和传递资讯

      所有作品版权归原创作者所有，与本站立场无关，如不慎侵犯了你的权益，请联系我们告知，我们将做删除处理！