ESKF_Error-state kinematics for IMU-driven systems

0.引言

经常查，总结一下。推导看原论文。

1.System kinematics in continuous time

1.1. The true-state kinematics

$$\begin{array}{rl}{\dot{\mathbf{p}}}_t& ={\mathbf{v}}_t\\ {\dot{\mathbf{v}}}_t& ={\mathbf{R}}_t\left({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n\right)+{\mathbf{g}}_t\\ {\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t\otimes \left({\mathbf{\omega }}_m-{\mathbf{\omega }}_{bt}-{\mathbf{\omega }}_n\right)\\ {\dot{\mathbf{a}}}_{bt}& ={\mathbf{a}}_w\\ {\dot{\mathbf{\omega }}}_{bt}& ={\mathbf{\omega }}_w\\ {\dot{\mathbf{g}}}_t& =0\end{array}$$
${\mathbf{g}}_t$ , $z$轴朝上。

1.2. The nominal-state kinematics

$$\begin{array}{rl}\dot{\mathbf{p}}& =\mathbf{v}\\ \dot{\mathbf{v}}& =\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{a}}_b\right)+\mathbf{g}\\ \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \left({\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right)\\ {\dot{\mathbf{a}}}_b& =0\\ {\dot{\mathbf{\omega }}}_b& =0\\ \dot{\mathbf{g}}& =0.\end{array}$$

$\mathbf{\omega }$ 为body下测量, $\mathbf{q}$ 为body到global的变换,故为local角速度更新方式.

1.3. The error-state kinematics

推导见论文。
$$\begin{array}{rl}\dot{\delta \mathbf{p}}& =\delta \mathbf{v}\\ \dot{\delta }\mathbf{v}& =-\mathbf{R}{\left[{\mathbf{a}}_m-{\mathbf{a}}_b\right]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-\mathbf{R}{\mathbf{a}}_n\\ \dot{\delta \mathbf{\theta }}& =-{\left[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n\\ \dot{\delta }{\dot{\mathbf{a}}}_b& ={\mathbf{a}}_w\\ \dot{\delta }{\dot{\mathbf{\omega }}}_b& ={\mathbf{\omega }}_w\\ \dot{\delta }\mathbf{g}& =0.\end{array}$$

进一步假设加速度计噪声为白噪声，相互独立，则$${\mathbf{a}}_n\leftarrow \mathbf{R}{\mathbf{a}}_n$$对均值和协方差均没有影响，故：
$$\delta \dot{\mathbf{v}}=-\mathbf{R}{\left[{\mathbf{a}}_m-{\mathbf{a}}_b\right]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-{\mathbf{a}}_n$$

对于 JPL：$$\dot{\delta }\mathbf{\theta }={\left[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n$$

2.System kinematics in discrete time

Integration needs to be done for the following sub-systems:

1. The nominal state.
2. The error-state.
   (a) The deterministic part: state dynamics and control.
   (b) The stochastic part: noise and perturbations.

更新方式

2.1.The nominal state kinematics

p ← p + v Δ t + 1 2 ( R ( a m − a b ) + g ) Δ t 2 v ← v + ( R ( a m − a b ) + g ) Δ t q ← q ⊗ q { ( ω m − ω b ) Δ t } a b ← a b ω b ← ω b g ← g \begin{aligned} \mathbf{p} & \leftarrow \mathbf{p}+\mathbf{v} \Delta t+\frac{1}{2}\left(\mathbf{R}\left(\mathbf{a}_{m}-\mathbf{a}_{b}\right)+\mathbf{g}\right) \Delta t^{2} \\ \mathbf{v} & \leftarrow \mathbf{v}+\left(\mathbf{R}\left(\mathbf{a}_{m}-\mathbf{a}_{b}\right)+\mathbf{g}\right) \Delta t \\ \mathbf{q} & \leftarrow \mathbf{q} \otimes \mathbf{q}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} \\ \mathbf{a}_{b} & \leftarrow \mathbf{a}_{b} \\ \boldsymbol{\omega}_{b} & \leftarrow \boldsymbol{\omega}_{b} \\ \mathbf{g} & \leftarrow \mathbf{g} \end{aligned} pvqab​ωb​g​←p+vΔt+21​(R(am​−ab​)+g)Δt2←v+(R(am​−ab​)+g)Δt←q⊗q{(ωm​−ωb​)Δt}←ab​←ωb​←g​

2.2.The error-state kinematics

δ p ← δ p + δ v Δ t δ v ← δ v + ( − R [ a m − a b ] × δ θ − R δ a b + δ g ) Δ t + v i δ θ ← R ⊤ { ( ω m − ω b ) Δ t } δ θ − δ ω b Δ t + θ i δ a b ← δ a b + a i δ ω b ← δ ω b + ω i δ g ← δ g \begin{aligned} \delta \mathbf{p} & \leftarrow \delta \mathbf{p}+\delta \mathbf{v} \Delta t \\ \delta \mathbf{v} & \leftarrow \delta \mathbf{v}+\left(-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}+\delta \mathbf{g}\right) \Delta t+\mathbf{v}_{\mathbf{i}} \\ \delta \boldsymbol{\theta} & \leftarrow \mathbf{R}^{\top}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b} \Delta t+\boldsymbol{\theta}_{\mathbf{i}} \\ \delta \mathbf{a}_{b} & \leftarrow \delta \mathbf{a}_{b}+\mathbf{a}_{\mathbf{i}} \\ \delta \boldsymbol{\omega}_{b} & \leftarrow \delta \boldsymbol{\omega}_{b}+\boldsymbol{\omega}_{\mathbf{i}} \\ \delta \mathbf{g} & \leftarrow \delta \mathbf{g} \end{aligned} δpδvδθδab​δωb​δg​←δp+δvΔt←δv+(−R[am​−ab​]×​δθ−Rδab​+δg)Δt+vi​←R⊤{(ωm​−ωb​)Δt}δθ−δωb​Δt+θi​←δab​+ai​←δωb​+ωi​←δg​

其中， ${\mathbf{v}}_i,{\theta }_i,{\mathbf{a}}_i,{\omega }_i$ 为作用于速度、方向和bias估计的随机值，为高斯白噪声。其均值为零，协方差矩阵为${\mathbf{a}}_n,{\omega }_n,{\mathbf{a}}_w,{\omega }_w$ 协方差积分所得。

$$\begin{array}{ll}{\mathbf{V}}_{\mathbf{i}}={\sigma }_{{\tilde{\mathbf{a}}}_n}^2\Delta t^2\mathbf{I}& \left[{\mathrm{m}}^2/{\mathrm{s}}^2\right]\\ {\Theta }_{\mathbf{i}}={\sigma }_{{\tilde{\mathbf{\omega }}}_n}\Delta t^2\mathbf{I}& \left[{\mathrm{r}\mathrm{a}\mathrm{d}}^2\right]\\ {\mathbf{A}}_{\mathbf{i}}={\sigma }_{{\mathbf{a}}_w}^2\Delta t\mathbf{I}& \left[{\mathrm{m}}^2/{\mathrm{s}}^4\right]\\ {\mathbf{\Omega }}_{\mathbf{i}}={\sigma }_{{\mathbf{\omega }}_w}^2\Delta t\mathbf{I}& \left[{\mathrm{r}\mathrm{a}\mathrm{d}}^2/{\mathrm{s}}^2\right]\end{array}$$

其中的方差则为测量计算所得。

2.3.The error-state Jacobian and perturbation matrices

$$\mathbf{x}=\left[\begin{array}{c}\mathbf{p}\\ \mathbf{v}\\ \mathbf{q}\\ {\mathbf{a}}_b\\ {\mathbf{\omega }}_b\\ \mathbf{g}\end{array}\right],\delta \mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathbf{\theta }\\ \delta {\mathbf{a}}_b\\ \delta {\mathbf{\omega }}_b\\ \delta \mathbf{g}\end{array}\right],{\mathbf{u}}_m=\left[\begin{array}{l}{\mathbf{a}}_m\\ {\mathbf{\omega }}_m\end{array}\right],\mathbf{i}=\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{i}}\\ {\mathbf{\theta }}_{\mathbf{i}}\\ {\mathbf{a}}_{\mathbf{i}}\\ {\mathbf{\omega }}_{\mathbf{i}}\end{array}\right]$$
The error-state system is now:

$$\delta \mathbf{x}\leftarrow f\left(\mathbf{x},\delta \mathbf{x},{\mathbf{u}}_m,\mathbf{i}\right)={\mathbf{F}}_{\mathbf{x}}\left(\mathbf{x},{\mathbf{u}}_m\right)\cdot \delta \mathbf{x}+{\mathbf{F}}_{\mathbf{i}}\cdot \mathbf{i}$$

The ESKF prediction equations are written:
$$\begin{array}{rl}\hat{\delta }\mathbf{x}& \leftarrow {\mathbf{F}}_{\mathbf{x}}\left(\mathbf{x},{\mathbf{u}}_m\right)\cdot \widehat{\delta \mathbf{x}}\end{array}\text{\hspace{1em}(1)}$$
$$\begin{array}{rl}\mathbf{P}& \leftarrow {\mathbf{F}}_{\mathbf{x}}\mathbf{P}{\mathbf{F}}_{\mathbf{x}}^{\top }+{\mathbf{F}}_{\mathbf{i}}{\mathbf{Q}}_{\mathbf{i}}{\mathbf{F}}_{\mathbf{i}}^{\top }\end{array}\text{\hspace{1em}(2)}$$

其中，

F x = ∂ f ∂ δ x ∣ x , u m = [ I I Δ t 0 0 0 0 0 I − R [ a m − a b ] × Δ t − R Δ t 0 I Δ t 0 0 R ⊤ { ( ω m − ω b ) Δ t } 0 − I Δ t 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] \mathbf{F}_{\mathbf{x}}=\left.\frac{\partial f}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}, \mathbf{u}_{m}}=\left[\begin{array}{cccccc} \mathbf{I} & \mathbf{I} \Delta t & 0 & 0 & 0 & 0 \\ 0 & \mathbf{I} & -\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \Delta t & -\mathbf{R} \Delta t & 0 & \mathbf{I} \Delta t \\ 0 & 0 & \mathbf{R}^{\top}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} & 0 & -\mathbf{I} \Delta t & 0 \\ 0 & 0 & 0 & \mathbf{I} & 0 & 0 \\ 0 & 0 & 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & 0 & 0 & \mathbf{I} \end{array}\right] Fx​=∂δx∂f​∣∣∣∣​x,um​​=⎣⎢⎢⎢⎢⎢⎢⎡​I00000​IΔtI0000​0−R[am​−ab​]×​ΔtR⊤{(ωm​−ωb​)Δt}000​0−RΔt0I00​00−IΔt0I0​0IΔt000I​⎦⎥⎥⎥⎥⎥⎥⎤​$${\mathbf{F}}_{\mathbf{i}}={\frac{\partial f}{\partial \mathbf{i}}|}_{\mathbf{x},{\mathbf{u}}_m}=\left[\begin{array}{llll}0& 0& 0& 0\\ \mathbf{I}& 0& 0& 0\\ 0& \mathbf{I}& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\\ 0& 0& 0& 0\end{array}\right],{\mathbf{Q}}_{\mathbf{i}}=\left[\begin{array}{cccc}{\mathbf{V}}_{\mathbf{i}}& 0& 0& 0\\ 0& {\Theta }_{\mathbf{i}}& 0& 0\\ 0& 0& {\mathbf{A}}_{\mathbf{i}}& 0\\ 0& 0& 0& {\mathbf{\Omega }}_{\mathbf{i}}\end{array}\right].$$

3.Fusing IMU with complementary sensory data

观测方程：
$$\mathbf{y}=h\left({\mathbf{x}}_t\right)+v$$
There are a myriad of possibilities, the most popular ones being GPS + IMU, monocular vision + IMU, and stereo vision + IMU. While the IMU information has served so far to make predictions to the ESKF, this other information is used to correct the filter, and thus observe the IMU bias errors. The correction consists of three steps:

1. observation of the error-state via filter correction,
2. injection of the observed errors into the nominal state, and
3. reset of the error-state.

3.1.Observation of the error state via filter correction

$$\begin{array}{rl}\mathbf{K}& =\mathbf{P}{\mathbf{H}}^{\top }{\left(\mathbf{H}\mathbf{P}{\mathbf{H}}^{\top }+\mathbf{V}\right)}^{-1}\end{array}\text{\hspace{1em}(3)}$$$$\begin{array}{rl}\widehat{\delta \mathbf{x}}& \leftarrow \mathbf{K}\left(\mathbf{y}-h\left({\hat{\mathbf{x}}}_t\right)\right)\end{array}\text{\hspace{1em}(4)}$$$$\begin{array}{rl}\mathbf{P}& \leftarrow (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}\end{array}\text{\hspace{1em}(5)}$$

---

$${\mathbf{H}\equiv \frac{\partial h}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}$$$${\hat{\mathbf{x}}}_t=\mathbf{x}\oplus \hat{\delta }\mathbf{x}$$
更新到普通状态。

3.2.Jacobian computation for the filter correction

$${\mathbf{H}\triangleq \frac{\partial h}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}={{\frac{\partial h}{\partial {\mathbf{x}}_t}|}_{\mathbf{x}}\frac{\partial {\mathbf{x}}_t}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}={\mathbf{H}}_{\mathbf{x}}{\mathbf{X}}_{\delta \mathbf{x}}$$
其中，
$${{\mathbf{H}}_{\mathbf{x}}\triangleq \frac{\partial h}{\partial {\mathbf{x}}_t}|}_{\mathbf{x}}$$

为观测方程对状态量的偏导，与实际传感器相关。第二部分 为true-state 相对于 error-state 的雅可比：

$${\mathbf{X}}_{\delta \mathbf{x}}=\left[\begin{array}{cccccc}\frac{\partial (\mathbf{p}+\delta \mathbf{p})}{\partial \delta \mathbf{p}}& 0& 0& 0& 0& 0\\ 0& \frac{\partial (\mathbf{v}+\delta \mathbf{v})}{\partial \delta \mathbf{v}}& 0& 0& 0& 0\\ 0& 0& \frac{\partial (\mathbf{q}\otimes \delta \mathbf{q})}{\partial \delta \mathbf{\theta }}& 0& 0& 0\\ 0& 0& 0& \frac{\partial \left({\mathbf{a}}_b+\delta {\mathbf{a}}_b\right)}{\partial \delta {\mathbf{a}}_b}& 0& 0\\ 0& 0& 0& 0& \frac{\partial \left({\mathbf{\omega }}_b+\delta {\mathbf{\omega }}_b\right)}{\partial \delta {\mathbf{\omega }}_b}& 0\\ 0& 0& 0& 0& 0& \frac{\partial (\mathbf{g}+\delta \mathbf{g})}{\partial \delta \mathbf{g}}\end{array}\right]$$
$${{\mathbf{X}}_{\delta \mathbf{x}}\triangleq \frac{\partial {\mathbf{x}}_t}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}=\left[\begin{array}{ccc}{\mathbf{I}}_6& 0& 0\\ 0& {\mathbf{Q}}_{\delta \mathbf{\theta }}& 0\\ 0& 0& {\mathbf{I}}_9\end{array}\right]$$
其中，$$\begin{array}{rl}{{\mathbf{Q}}_{\delta \mathbf{\theta }}\triangleq \frac{\partial (\mathbf{q}\otimes \delta \mathbf{q})}{\partial \delta \mathbf{\theta }}|}_{\mathbf{q}}& ={{\frac{\partial (\mathbf{q}\otimes \delta \mathbf{q})}{\partial \delta \mathbf{q}}|}_{\mathbf{q}}\frac{\partial \delta \mathbf{q}}{\partial \delta \mathbf{\theta }}|}_{\delta \hat{\theta }=0}\\ & ={{\frac{\partial \left([\mathbf{q}{]}_L\delta \mathbf{q}\right)}{\partial \delta \mathbf{q}}|}_{\mathbf{q}}\frac{\partial \left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathbf{\theta }\end{array}\right]}{\partial \delta \mathbf{\theta }}|}_{\delta \mathbf{\theta }=0}\\ & =[\mathbf{q}{]}_L\frac{1}{2}\left[\begin{array}{ccc}0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\\ {\mathbf{Q}}_{\delta \mathbf{\theta }}=\frac{1}{2}& \left[\begin{array}{ccc}-q_x& -q_y& -q_z\\ q_w& -q_z& q_y\\ q_z& q_w& -q_x\\ -q_y& q_x& q_w\end{array}\right]\end{array}$$

3.3.Injection of the observed error into the nominal state

After the ESKF update, the nominal state gets updated with the observed error state using the appropriate compositions:
p ← p + δ p ^ v ← v + δ v ^ q ← q ⊗ q { δ θ ^ } a b ← a b + δ a b ^ ω b ← ω b + δ ω ^ b g ← g + δ g ^ \begin{aligned} \mathbf{p} & \leftarrow \mathbf{p}+\hat{\delta \mathbf{p}} \\ \mathbf{v} & \leftarrow \mathbf{v}+\hat{\delta \mathbf{v}} \\ \mathbf{q} & \leftarrow \mathbf{q} \otimes \mathbf{q}\{\hat{\delta \boldsymbol{\theta}}\} \\ \mathbf{a}_{b} & \leftarrow \mathbf{a}_{b}+\delta \hat{\mathbf{a}_{b}} \\ \boldsymbol{\omega}_{b} & \leftarrow \boldsymbol{\omega}_{b}+\delta \hat{\boldsymbol{\omega}}_{b} \\ \mathbf{g} & \leftarrow \mathbf{g}+\hat{\delta \mathbf{g}} \end{aligned} pvqab​ωb​g​←p+δp^​←v+δv^←q⊗q{δθ^}←ab​+δab​^​←ωb​+δω^b​←g+δg^​​

3.4.ESKF reset

Let us call the error reset function $g()$. It is written as follows,

$$\delta \mathbf{x}\leftarrow g(\delta \mathbf{x})=\delta \mathbf{x}\ominus \widehat{\delta \mathbf{x}}$$其中，$$\begin{array}{rl}\widehat{\delta \mathbf{x}}& \leftarrow 0\\ \mathbf{P}& \leftarrow \mathbf{G}\mathbf{P}{\mathbf{G}}^{\top }.\end{array}$$$${\mathbf{G}\triangleq \frac{\partial g}{\partial \delta \mathbf{x}}|}_{\delta \mathbf{x}}.$$

和前面的 Jacobian 类似，除了四元数项外，其他均为单位矩阵：

$$\mathbf{G}=\left[\begin{array}{ccc}{\mathbf{I}}_6& 0& 0\\ 0& \mathbf{I}-{\left[\begin{array}{c}\frac{1}{2}\widehat{\delta \mathbf{\theta }}\end{array}\right]}_{\times }& 0\\ 0& 0& {\mathbf{I}}_9\end{array}\right]$$

多数系统里实际忽略了 $\hat{\delta }\theta$ 项， $\mathbf{G}={\mathbf{I}}_{18\ast 18}$.

3.5.Jacobian of the reset operation with respect to the orientation error

• 复位操作时，true-oritentation 不会改变，${\mathbf{q}}_t^{+}={\mathbf{q}}_t$:$${\mathbf{q}}^{+}\otimes \delta {\mathbf{q}}^{+}=\mathbf{q}\otimes \delta \mathbf{q}$$

• 观测误差均值已经融合入标称状态：$${\mathbf{q}}^{+}=\mathbf{q}\otimes \widehat{\delta \mathbf{q}}$$
  结合两式得：
  $$\delta {\mathbf{q}}^{+}={\mathbf{q}}^{+}\otimes \mathbf{q}\otimes \delta \mathbf{q}=(\mathbf{q}\otimes \widehat{\delta \mathbf{q}}{)}^{\ast }\otimes \mathbf{q}\otimes \delta \mathbf{q}={\widehat{\delta \mathbf{q}}}^{\ast }\otimes \delta \mathbf{q}=[\widehat{\delta \mathbf{q}}{]}_L^{\ast }\cdot \delta \mathbf{q}$$

推导见论文，结果：$$\frac{\partial \delta {\mathbf{\theta }}^{+}}{\partial \delta \mathbf{\theta }}=\mathbf{I}-{\left[\frac{1}{2}\widehat{\delta \mathbf{\theta }}\right]}_{\times }$$