The error state in MSCKF

2020-07-16 本文已影响0人轻骑兵1390

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}$