The Detail of The Error State in MSCKF

1. Rotation

The error-state rotation propagation is marked as (14) in msckf2.0,
$^{I} \tilde{\boldsymbol{\theta}}_{\ell+1 \mid \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} \tag{1}$
We give the detail of (1) in this section and firstly simplify the sample from to:

Considering , we give the following derivation as:
$\begin{aligned} (I-[\phi_{l+1}]_\times)R_{l+1} &= (I-[\phi_{\Delta}]_\times)R_\Delta \cdot (I-[\phi_l]_\times)R_l \\ R_{l+1}-[\phi_{l+1}]_\times R_{l+1} &= (R_\Delta-[\phi_{\Delta}]_\times R_\Delta) \cdot (R_l-[\phi_l]_\times R_l) \\ &= R_\Delta R_l-[\phi_{\Delta}]_\times R_\Delta R_l -R_\Delta[\phi_l]_\times R_l + \boxed{[\phi_{\Delta}]_\times R_\Delta [\phi_l]_\times R_l}\\ &\approx R_\Delta R_l-[\phi_{\Delta}]_\times R_\Delta R_l -R_\Delta[\phi_l]_\times R_l \\ &= R_{l+1}-[\phi_{\Delta}]_\times R_\Delta R_l -R_\Delta[\phi_l]_\times R_l \end{aligned}$

$\begin{aligned} [\phi_{l+1}]_\times R_{l+1} &= [\phi_{\Delta}]_\times R_\Delta R_l +R_\Delta[\phi_l]_\times R_l \\ [\phi_{l+1}]_\times &= [\phi_{\Delta}]_\times +R_\Delta[\phi_l]_\times R_\Delta^T\\ &= [\phi_{\Delta}]_\times + [R_\Delta\cdot \phi_l]_\times\\ \end{aligned}$
The result is

2. Velocity

We simplify the velocity function to

We use , where is the estimated value and usually calculated by the measurement value. is the error state.

$\begin{aligned} \hat{v}_{l+1} + \tilde{v}_{l+1} &= (\hat{v}_{l} + \tilde{v}_{l}) + g\Delta t + ((I-[\tilde{\theta}_l]_\times)\hat{R}_{l})^T\cdot (\hat{s}_l + \tilde{s}_l) \\ \hat{v}_{l+1} + \tilde{v}_{l+1} &= (\hat{v}_{l} + \tilde{v}_{l}) + g\Delta t + ((I-[\tilde{\theta}_l]_\times)\hat{R}_{l})^T\cdot (\hat{s}_l + \tilde{s}_l) \\ &= (\hat{v}_{l} + \tilde{v}_{l}) + g\Delta t + (\hat{R}_{l}^T - \hat{R}_{l}^T([\tilde\theta_l]_\times)^T) \cdot (\hat{s}_l + \tilde{s}_l) \\ &= (\hat{v}_{l} + \tilde{v}_{l}) + g\Delta t + (\hat{R}_{l}^T \hat{s}_l + \hat{R}_{l}^T\tilde{s}_l - \hat{R}_{l}^T([\tilde\theta_l]_\times)^T \hat{s}_l - \hat{R}_{l}^T([\tilde\theta_l]_\times)^T \tilde{s}_l) \\ \end{aligned}$
The element has two differential value thus is deleted. Then
$\begin{aligned} \cancel{\hat{v}_{l+1}} + \tilde{v}_{l+1} &= \cancel{\hat{v}_{l}} + \tilde{v}_{l} +\cancel{g\Delta t} + \cancel{\hat{R}_{l}^T \hat{s}_l} + \hat{R}_{l}^T\tilde{s}_l - \hat{R}_{l}^T([\tilde\theta_l]_\times)^T \hat{s}_l\\ \tilde{v}_{l+1} &= \tilde{v}_{l} + \hat{R}_{l}^T\tilde{s}_l - \hat{R}_{l}^T([\tilde\theta_l]_\times)^T \hat{s}_l\\ \end{aligned}$
Based on and , we finally get

10. Question

10.1 ? ?

The main different between Hamilton and JPL quaternions is

Hamilton, , right-hand.
JPL, , left-hand.

The difference between and is also related to Hamilton and JPL quaternions.

The Detail of The Error State in MSCKF

1. Rotation

2. Velocity

10. Question

10.1 ? ?

你可能感兴趣的:(The Detail of The Error State in MSCKF)