久久开心AV,日本中文一区二区

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 $^{I_{l+1}}_{G}{R} = \space^{I_{l+1}}_{I_l}{R} \cdot \space^{I_{l}}_{G}{R}$ to:
$R_{l+1} = R_{\Delta}R_xl$
Considering ${R} \approx(I_3-[\phi_\times]) \cdot \hat{R}$ , 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
$\phi_{l+1} = \phi_{\Delta} + R_\Delta\cdot \phi_l$

2. Velocity

We simplify the velocity function to
$v_{l+1} = v_{l} + g\Delta t + R^T_{l}\cdot s_l$

We use $x = \hat{x} + \tilde{x}$ , where $\hat{x}$ is the estimated value and usually calculated by the measurement value. $\tilde{x}$ 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 $\hat{R}_{l}^T([\tilde\theta_l]_\times)^T \tilde{s}_l$ 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 $(u_\times)^T = -u_\times$ and $u_\times v = -v_\times u$ , we finally get
$\tilde{v}_{l+1} = \tilde{v}_{l} + \hat{R}_{l}^T\tilde{s}_l - \hat{R}_{l}^T[\hat{s}_l]_\times \tilde{\theta}_l$