【ESKF】（一）IMU运动学方程离散化

参考：《Quaternion kinematics for error-state kalman filter》

1. 三种状态

1. true-state
   真实状态值，包含高斯噪声，用于EKF作为状态变量使用
2. nominal-state
   • 无（不考虑）噪声的状态值，可以是估计值（不考虑噪声）、预测值（不考虑噪声）
   • 在预测阶段，由高频的IMU测量值预测得到当前的nominal-state $\mathbf{x}$，nominal state没有考虑噪声项，因此会存在累计误差，这些误差被堆积到error-state $\delta \mathbf{x}$中，
3. error-state
   • 误差状态，符合零均值高斯分布，用于ESKF
     true-state = nominal-state + error-state
   • ESKF中dynamic, control, measurement matrices由nominal-state计算得到

2. 连续时间的系统运动学（System kinematics in continuous time）

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}$$
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{\mathrm{g}}& =0\end{array}$$
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{Ra}}_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 \mathbf{a}}}_b& ={\mathbf{a}}_w\\ \delta {\dot{\mathbf{\omega }}}_b& ={\mathbf{\omega }}_w\\ \dot{\delta \mathbf{g}}& =0\end{array}$$

3. 离散时间的系统运动学（System kinematics in discrete time）

连续时间的系统运动学方程是微分方程，离散化后转换成差分方程，所谓离散化，其实本质就是积分

1. The nominal state kinematics
   此差分方程用于预测系统状态
   p←p+vΔt+12(R(am−ab)+g)Δt2v←v+(R(am−ab)+g)Δtq←q⊗q{(ωm−ωb)Δt}ab←abωb←ωbg←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. The error-state kinematics
   此差分方程用于协方差传递
   δp←δp+δvΔtδv←δv+(−R[am−ab]×δθ−Rδab+δg)Δt+viδθ←R⊤{(ωm−ωb)Δt}δθ−δωbΔt+θiδab←δab+aiδω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​

4. 离散时间的error-state kinematics方程推导

1. 第一步：写出连续时间的true-state kinematics
   x˙=f(x,u,w)u~∼N{0,Uc},wc∼N{0,Wc} \dot{\mathbf{x}}=f(\mathbf{x}, \mathbf{u}, \mathbf{w}) \\ \tilde{\mathbf{u}} \sim \mathcal{N}\left\{0, \mathbf{U}^{c}\right\} \quad, \quad \mathbf{w}^{c} \sim \mathcal{N}\left\{0, \mathbf{W}^{c}\right\} x˙=f(x,u,w)u~∼N{0,Uc},wc∼N{0,Wc}

   注意，区分两种噪声模型：

   • 控制信号噪声$\tilde{\mathbf{u}}$
     此噪声在采样时间段内认为是常量
   • 随机扰动噪声${\mathbf{w}}^c$
     跟采样无关，时刻以高斯分布的形态改变（时变量），布朗运动

2. 第二步：写出连续时间的error-state kinematics（一阶泰勒展开）
   $$\begin{gathered} \dot{\delta }\mathbf{x}=\mathbf{A}\delta \mathbf{x}+\mathbf{B}\tilde{\mathbf{u}}+\mathbf{Cw} \\ {\mathbf{A}\triangleq \frac{\partial f}{\partial \delta \mathbf{x}}|}_{\mathbf{x},{\mathbf{u}}_m},{\mathbf{B}\triangleq \frac{\partial f}{\partial \tilde{\mathbf{u}}}|}_{\mathbf{x},{\mathbf{u}}_m},{\mathbf{C}\triangleq \frac{\partial f}{\partial \mathbf{w}}|}_{\mathbf{x},{\mathbf{u}}_m} \end{gathered}$$

3. 第三步：积分得到离散时间的error-state kinematics
   $$\begin{array}{rl}\delta {\mathbf{x}}_{n+1}& =\delta {\mathbf{x}}_n+{\int }_{n\Delta t}^{(n+1)\Delta t}\left(\mathbf{A}\delta \mathbf{x}(\tau )+\mathbf{B}\tilde{\mathbf{u}}(\tau )+\mathbf{C}{\mathbf{w}}^c(\tau )\right)d\tau \\ & =\delta {\mathbf{x}}_n+{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{A}\delta \mathbf{x}(\tau )d\tau +{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{B}\tilde{\mathbf{u}}(\tau )d\tau +{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{C}{\mathbf{w}}^c(\tau )d\tau \end{array}$$

   分三部分进行积分：

   • dynamic部分
     $$\delta {\mathbf{x}}_n+{\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{A}\delta \mathbf{x}(\tau )d\tau =\Phi \cdot \delta {\mathbf{x}}_n$$

     其中，$\Phi =e^{\mathbf{A}\Delta t}$可以通过闭式积分或者近似积分的方式得到

   • 一旦被采样，在积分阶段采样噪声不再改变（该噪声已经在采样时成为确定），相当于常量进行积分
     $${\int }_{n\Delta t}^{(n+1)\Delta t}\mathbf{B}\tilde{\mathbf{u}}(\tau )d\tau =\mathbf{B}\Delta t{\tilde{\mathbf{u}}}_n$$

   • bias随机游走噪声属于布朗运动，在积分阶段也是时变的，所以需要对高斯分布进行积分：
     wn≜∫nΔt(n+1)Δtw(τ)dτ,wn∼N{0,W}, with W=WcΔt \mathbf{w}_{n} \triangleq \int_{n \Delta t}^{(n+1) \Delta t} \mathbf{w}(\tau) d \tau \quad, \quad \mathbf{w}_{n} \sim \mathcal{N}\{0, \mathbf{W}\} \quad, \quad \text { with } \mathbf{W}=\mathbf{W}^{c} \Delta t wn​≜∫nΔt(n+1)Δt​w(τ)dτ,wn​∼N{0,W}, with W=WcΔt

4. 最终结果：
   $$\begin{array}{ccc}\text{ Description }& \text{ Continuous time }t& \text{ Discrete time }n\Delta t\\ \text{ state }& \dot{\mathbf{x}}=f^c(\mathbf{x},\mathbf{u},\mathbf{w})& {\mathbf{x}}_{n+1}=f\left({\mathbf{x}}_n,{\mathbf{u}}_n,{\mathbf{w}}_n\right)\\ \text{ error-state }& \dot{\delta \mathbf{x}}=\mathbf{A}\delta \mathbf{x}+\mathbf{B}\tilde{\mathbf{u}}+\mathbf{Cw}& \delta {\mathbf{x}}_{n+1}={\mathbf{F}}_{\mathbf{x}}\delta {\mathbf{x}}_n+{\mathbf{F}}_{\mathbf{u}}{\tilde{\mathbf{u}}}_n+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_n\\ \text{ system/ transition matrix }& \mathbf{A}& {\mathbf{F}}_{\mathbf{x}}=\Phi =e^{\mathbf{A}\Delta t}\\ \text{ control matrix }& \mathbf{B}& {\mathbf{F}}_{\mathbf{u}}=\mathbf{B}\Delta t\\ \text{ perturbation matrix }& \mathbf{C}& {\mathbf{F}}_{\mathbf{w}}=\mathbf{C}\\ \text{ control covariance }& {\mathbf{U}}^c& \mathbf{U}={\mathbf{U}}^c\\ \text{ perturbation covariance }& {\mathbf{W}}^c& \mathbf{W}={\mathbf{W}}^c\Delta t\end{array}$$

   δxn+1=Fxδxn+Fuu~n+Fwwnu~n∼N{0,U},wn∼N{0,W} \delta \mathbf{x}_{n+1}=\mathbf{F}_{\mathbf{x}} \delta \mathbf{x}_{n}+\mathbf{F}_{\mathbf{u}} \tilde{\mathbf{u}}_{n}+\mathbf{F}_{\mathbf{w}} \mathbf{w}_{n} \\ \tilde{\mathbf{u}}_{n} \sim \mathcal{N}\{0, \mathbf{U}\} \quad, \quad \mathbf{w}_{n} \sim \mathcal{N}\{0, \mathbf{W}\} δxn+1​=Fx​δxn​+Fu​u~n​+Fw​wn​u~n​∼N{0,U},wn​∼N{0,W}

   EKF预测阶段的两个方程：
   $$\begin{array}{rl}{\widehat{\delta \mathbf{x}}}_{n+1}& ={\mathbf{F}}_{\mathbf{x}}\hat{\delta }{\mathbf{x}}_n\\ {\mathbf{P}}_{n+1}& ={\mathbf{F}}_{\mathbf{x}}{\mathbf{P}}_n{\mathbf{F}}_{\mathbf{x}}^{\top }+{\mathbf{F}}_{\mathbf{u}}\mathbf{U}{\mathbf{F}}_{\mathbf{u}}^{\top }+{\mathbf{F}}_{\mathbf{w}}\mathbf{W}{\mathbf{F}}_{\mathbf{w}}^{\top }\\ & =e^{\mathbf{A}\Delta t}{\mathbf{P}}_n{\left(e^{\mathbf{A}\Delta t}\right)}^{\top }+\Delta t^2\mathbf{B}{\mathbf{U}}^c{\mathbf{B}}^{\top }+\Delta t\mathbf{C}{\mathbf{W}}^c{\mathbf{C}}^{\top }\end{array}$$

   注意观察：

   • the dynamic error term is exponential
   • the measurement error term is quadratic
   • and the perturbation error term is linear

5. ESKF中需要进行两次积分

Integration needs to be done for the following sub-systems

• The nominal state
• The error-state
  • The deterministic part: state dynamics and control
  • The stochastic part: noise and perturbations