流型上的Kalman Filter

连续时间系统运动学

The true-state kinematics

$$\begin{array}{rl}\dot{\mathbf{p}}& =\mathbf{v}\\ \dot{\mathbf{v}}& =\mathbf{a}\\ \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega }\\ {\dot{\mathbf{b}}}_a& ={\mathbf{n}}_a\\ {\dot{\mathbf{b}}}_{\mathbf{\omega }}& ={\mathbf{n}}_{\mathbf{\omega }}\\ \dot{\mathbf{g}}& =0\end{array}$$
真值状态的观测来源于IMU测量的加速度和角速度(IMU系)
$$\begin{array}{rl}& \{\begin{array}{l}{\mathbf{a}}_m={\mathbf{R}}^T\left(\mathbf{a}-\mathbf{g}\right)+{\mathbf{b}}_a+{\mathbf{n}}_a\\ {\mathbf{\omega }}_m=\mathbf{\omega }+{\mathbf{b}}_{\mathbf{w}}+{\mathbf{n}}_{\mathbf{\omega }}\end{array}\\ ⟹& \{\begin{array}{l}\mathbf{a}=\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)+\mathbf{g}\\ \mathbf{\omega }={\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}\end{array}\end{array}$$
代入可得

$$\begin{array}{rl}\dot{\mathbf{p}}& =\mathbf{v}\\ \dot{\mathbf{v}}& =\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a\right)+\mathbf{g}\\ \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \left({\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}\right)\\ {\dot{\mathbf{b}}}_a& ={\mathbf{n}}_{\mathbf{ba}}\\ {\dot{\mathbf{b}}}_{\mathbf{\omega }}& ={\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}\\ \dot{\mathbf{g}}& =0\end{array}\text{\hspace{1em}(true state)}$$
写成更简便的形式
$$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u},\mathbf{w})$$
其中$\mathbf{x}$为状态向量，$\mathbf{u}$为受噪声影响的IMU测量，$\mathbf{w}$为高斯白噪声扰动
x = [ p v q b a b ω g ] u = [ a m − n a ω m − n ω ] w = [ n b a n b ω ] \bold{x}=\begin{bmatrix}\bold{p}\\\bold{v}\\\bold{q}\\\bold{b_a}\\\bold{b}_{\bm{\omega}}\\\bold{g}\end{bmatrix}\qquad \bold{u}=\begin{bmatrix}\bold{a}_{m}-\bold{n_{a}}\\ \bm{\omega}_{m}-\bold{n_{\bm{\omega}}}\end{bmatrix}\qquad \bold{w}=\begin{bmatrix}\bold{n}_{\bold{ba}}\\\bold{n}_{\bold{b}{\bm{\omega}}}\end{bmatrix} x= ​pvqba​bω​g​ ​u=[am​−na​ωm​−nω​​]w=[nba​nbω​​]

The nominal-state kinematics

系统的名义态运动学方程对应于无噪声和扰动的系统模型。

$$\begin{array}{rl}\dot{\bar{\mathbf{p}}}& =\bar{\mathbf{v}}\\ \dot{\bar{\mathbf{v}}}& =\bar{\mathbf{R}}\left({\mathbf{a}}_m-{\bar{\mathbf{b}}}_a\right)+\bar{\mathbf{g}}\\ \dot{\bar{\mathbf{q}}}& =\frac{1}{2}\bar{\mathbf{q}}\otimes \left({\mathbf{\omega }}_m-{\bar{\mathbf{b}}}_{\mathbf{\omega }}\right)\\ {\dot{\bar{\mathbf{b}}}}_a& =0\\ {\dot{\bar{\mathbf{b}}}}_{\mathbf{\omega }}& =0\\ \dot{\bar{\mathbf{g}}}& =0\end{array}\text{\hspace{1em}(nominal state)}$$

The error-state kinematics

由$\mathbf{x}=\bar{\mathbf{x}}+\delta \mathbf{x}⟹\delta \mathbf{x}=\mathbf{x}-\bar{\mathbf{x}}$，其中+,-代指compose和decompose，根据变量类型采取不同的compose方式， 对于四元数
$$\mathbf{q}=\bar{\mathbf{q}}\otimes \delta \mathbf{q}$$
为了简便起见，我们将角速度分成大信号量项$\bar{\mathbf{\omega }}\triangleq {\mathbf{\omega }}_m-{\bar{\mathbf{b}}}_{\mathbf{\omega }}$和小信号量项$\delta \mathbf{\omega }=-\delta {\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}$
$$\mathbf{\omega }=\bar{\mathbf{\omega }}+\delta \mathbf{\omega }$$
于是
$$\begin{array}{rl}\dot{\mathbf{q}}=\dot{\bar{\mathbf{q}}}\otimes \delta \mathbf{q}+\bar{\mathbf{q}}\otimes \dot{\delta \mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \left({\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}\right)\\ \frac{1}{2}\bar{\mathbf{q}}\otimes \left({\mathbf{\omega }}_m-{\bar{\mathbf{b}}}_{\mathbf{\omega }}\right)\otimes \delta \mathbf{q}+\bar{\mathbf{q}}\otimes \dot{\delta \mathbf{q}}& =\frac{1}{2}\bar{\mathbf{q}}\otimes \delta \mathbf{q}\otimes \left({\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}\right)\\ ⟹2\dot{\delta \mathbf{q}}& =\delta \mathbf{q}\otimes \mathbf{\omega }-\bar{\mathbf{\omega }}\otimes \delta \mathbf{q}\\ & =\left([\mathbf{\omega }{]}_R-[\bar{\mathbf{\omega }}{]}_L\right)\delta \mathbf{q}\\ & =\left[\begin{array}{cc}0& -{\left(\mathbf{\omega }-\bar{\mathbf{\omega }}\right)}^T\\ \mathbf{\omega }-\bar{\mathbf{\omega }}& -{\left(\mathbf{\omega }+\bar{\mathbf{\omega }}\right)}^{\wedge }\end{array}\right]\delta \mathbf{q}\\ & =\left[\begin{array}{cc}0& -\delta {\mathbf{\omega }}^T\\ \delta \mathbf{\omega }& -{\left(2\overline{\mathbf{\omega }+\delta \mathbf{\omega }}\right)}^{\wedge }\end{array}\right]\delta \mathbf{q}\end{array}$$
对$\delta \mathbf{q}$进行小量近似， δ q = [ cos ⁡ ( ∥ δ θ ∥ / 2 ) δ θ ∥ δ θ ∥ sin ⁡ ( ∥ δ θ ∥ / 2 ) ] ≈ [ 1 δ θ / 2 ] + O ( ∥ δ θ ∥ 2 ) \delta \bold{q}=\begin{bmatrix}\cos(\Vert\delta \bm{\theta}\Vert/2)\\\dfrac{\delta\bm{\theta}}{\Vert\delta\bm{\theta}\Vert} \sin(\Vert\delta \bm{\theta}\Vert/2)\end{bmatrix}\approx\begin{bmatrix}1\\\delta \bm{\theta}/2\end{bmatrix}+O(\Vert\delta\bm{\theta}\Vert^2) δq= ​cos(∥δθ∥/2)∥δθ∥δθ​sin(∥δθ∥/2)​ ​≈[1δθ/2​]+O(∥δθ∥2)，于是
$$\begin{array}{rl}\left[\begin{array}{c}0\\ \dot{\delta \mathbf{\theta }}\end{array}\right]& =\left[\begin{array}{cc}0& -\delta {\mathbf{\omega }}^T\\ \delta \mathbf{\omega }& -{\left(2\mathbf{\omega }+\delta \mathbf{\omega }\right)}^{\wedge }\end{array}\right]\left[\begin{array}{c}1\\ \delta \mathbf{\theta }/2\end{array}\right]+O\left({\Vert \delta \mathbf{\theta }\Vert }^2\right)\\ ⟹\dot{\delta \mathbf{\theta }}& =\delta \mathbf{\omega }-{\mathbf{\omega }}^{\wedge }\delta \mathbf{\theta }-\frac{1}{2}\delta {\mathbf{\omega }}^{\wedge }\delta \mathbf{\theta }+O(\Vert \delta \mathbf{\theta }{\Vert }^2)\end{array}$$
忽略二阶小量之后，可以得到
$$\begin{array}{rl}\dot{\delta \mathbf{\theta }}& =\delta \mathbf{\omega }-{\mathbf{\omega }}^{\wedge }\delta \mathbf{\theta }\\ & =-({\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}{)}^{\wedge }\delta \mathbf{\theta }-\delta {\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}\end{array}$$
对于SO(3)
$$\begin{array}{rl}\mathbf{R}& =\bar{\mathbf{R}}\delta \mathbf{R}\\ & =\bar{\mathbf{R}}\mathrm{Exp}\left(\delta \mathbf{\theta }\right)\\ & \approx \bar{\mathbf{R}}\left(\mathbf{I}+\delta {\mathbf{\theta }}^{\wedge }\right)+O\left({\Vert \delta \mathbf{\theta }\Vert }^2\right)\end{array}$$
其中$\delta \mathbf{\theta }$为$\delta \mathbf{R}$的李代数。
$$\begin{array}{rl}\dot{\delta \mathbf{v}}=\dot{\mathbf{v}}-\dot{\bar{\mathbf{v}}}& =\left[\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{b}}_{\mathbf{a}}-{\mathbf{n}}_a\right)+\mathbf{g}\right]-\left[\bar{\mathbf{R}}\left({\mathbf{a}}_m-{\bar{\mathbf{b}}}_a\right)+\bar{\mathbf{g}}\right]\\ & =\left[\mathbf{R}\left({\mathbf{a}}_m-{\bar{\mathbf{b}}}_a-\delta {\mathbf{b}}_a-{\mathbf{n}}_a\right)+\mathbf{g}\right]-\left[\bar{\mathbf{R}}\left({\mathbf{a}}_m-{\bar{\mathbf{b}}}_a\right)+\bar{\mathbf{g}}\right]\\ & =\left(\mathbf{R}-\bar{\mathbf{R}}\right)\left({\mathbf{a}}_m-{\bar{\mathbf{b}}}_a\right)-\mathbf{R}\left(\delta {\mathbf{b}}_{\mathbf{a}}+{\mathbf{n}}_a\right)+\delta \mathbf{g}\\ & =\bar{\mathbf{R}}\left[\mathrm{Exp}(\delta \mathbf{\theta })-\mathbf{I}\right]\left({\mathbf{a}}_m-{\bar{\mathbf{b}}}_a\right)-\bar{\mathbf{R}}\mathrm{Exp}(\delta \mathbf{\theta })\left(\delta {\mathbf{b}}_{\mathbf{a}}+{\mathbf{n}}_a\right)+\delta \mathbf{g}\\ & =\bar{\mathbf{R}}\delta {\mathbf{\theta }}^{\wedge }\left({\mathbf{a}}_m-{\bar{\mathbf{b}}}_a\right)-\bar{\mathbf{R}}\left(\mathbf{I}+\delta {\mathbf{\theta }}^{\wedge }\right)\left(\delta {\mathbf{b}}_{\mathbf{a}}+{\mathbf{n}}_a\right)+\delta \mathbf{g}\end{array}$$

忽略二阶小量$\delta \cdot \delta$或$\delta \cdot \mathbf{n}$，同时利用${\mathbf{a}}^{\wedge }\mathbf{b}=-{\mathbf{b}}^{\wedge }\mathbf{a}$可得
$$\dot{\delta \mathbf{v}}=\bar{\mathbf{R}}\left[-{\left({\mathbf{a}}_m-{\bar{\mathbf{b}}}_a\right)}^{\wedge }\delta \mathbf{\theta }-\delta {\mathbf{b}}_{\mathbf{a}}-{\mathbf{n}}_a\right]+\delta \mathbf{g}$$
我们假设加速度计噪声是白噪声、不相关的、各向同性的。
$$E({\mathbf{n}}_a)=0E({\mathbf{n}}_a{\mathbf{n}}_a^T)={\sigma }_a^2\mathbf{I}$$
也就是说，协方差椭球是一个以原点为中心的球体，这意味着它的均值和协方差矩阵在旋转时是不变的。
证明:$E(\mathbf{R}{\mathbf{n}}_a)=\mathbf{R}E({\mathbf{n}}_a)=0$ 和$E[(\mathbf{R}{\mathbf{n}}_a)(\mathbf{R}{\mathbf{n}}_a{)}^T]=\mathbf{R}E[{\mathbf{n}}_a{\mathbf{n}}_a^T]{\mathbf{R}}^T=\mathbf{R}{\sigma }_a^2\mathbf{I}{\mathbf{R}}^T={\sigma }_a^2\mathbf{I}$。然后我们可以重新定义加速度计噪声矢量：
$${\mathbf{n}}_a\leftarrow \mathbf{R}{\mathbf{n}}_a$$
其余误差状态量的微分方程可以很容易的得到，于是

$$\begin{array}{rl}\dot{\delta \mathbf{p}}& =\delta \mathbf{v}\\ \dot{\delta \mathbf{v}}& =\bar{\mathbf{R}}\left[-{\left({\mathbf{a}}_m-{\bar{\mathbf{b}}}_a\right)}^{\wedge }\delta \mathbf{\theta }-\delta {\mathbf{b}}_{\mathbf{a}}\right]+\delta \mathbf{g}-{\mathbf{n}}_a\\ \dot{\delta \mathbf{\theta }}& =-({\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}{)}^{\wedge }\delta \mathbf{\theta }-\delta {\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}\\ \dot{\delta {\mathbf{b}}_a}& ={\mathbf{n}}_{\mathbf{ba}}\\ \dot{\delta {\mathbf{b}}_{\mathbf{\omega }}}& ={\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}\\ \dot{\delta \mathbf{g}}& =0\end{array}\text{\hspace{1em}(error state)}$$

离散时间系统动力学

The nominal-state kinematics

$$\begin{array}{rl}\mathbf{p}& \leftarrow \mathbf{p}+\mathbf{v}\Delta t+\frac{1}{2}\left(\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{b}}_{\mathbf{a}}\right)+\mathbf{g}\right)\Delta t^2\\ \mathbf{v}& \leftarrow \mathbf{v}+\left(\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{b}}_{\mathbf{a}}\right)+\mathbf{g}\right)\Delta t\\ \mathbf{q}& \leftarrow \mathbf{q}\otimes \mathbf{q}\left\{\left({\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}\right)\Delta t\right\}\\ {\mathbf{b}}_{\mathbf{a}}& \leftarrow {\mathbf{b}}_{\mathbf{a}}\\ {\mathbf{b}}_{\mathbf{\omega }}& \leftarrow {\mathbf{b}}_{\mathbf{\omega }}\\ \mathbf{g}& \leftarrow \mathbf{g}\end{array}$$

The error-state kinematics

$$\begin{array}{rl}\delta \mathbf{p}& \leftarrow \delta \mathbf{p}+\delta \mathbf{v}\Delta t\\ \delta \mathbf{v}& \leftarrow \delta \mathbf{v}+\left(-\bar{\mathbf{R}}{\left({\mathbf{a}}_m-{\mathbf{b}}_{\mathbf{a}}\right)}^{\wedge }\delta \mathbf{\theta }-\bar{\mathbf{R}}\delta {\mathbf{b}}_{\mathbf{a}}+\delta \mathbf{g}\right)\Delta t+{\mathbf{v}}_i\\ \delta \mathbf{\theta }& \leftarrow {\mathbf{R}}^T\left\{\left({\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}\right)\Delta t\right\}\delta \mathbf{\theta }-{\mathbf{b}}_{\mathbf{\omega }}\Delta t+{\mathbf{\theta }}_i\\ \delta {\mathbf{b}}_a& \leftarrow \delta {\mathbf{b}}_{\mathbf{a}}+{\mathbf{a}}_i\\ \delta {\mathbf{b}}_{\mathbf{\omega }}& \leftarrow \delta {\mathbf{b}}_{\mathbf{\omega }}+{\mathbf{\omega }}_i\\ \delta \mathbf{g}& \leftarrow \delta \mathbf{g}\end{array}$$
其中$\delta \mathbf{\theta }$式的积分参照，${\mathbf{v}}_i,{\mathbf{\theta }}_i,{\mathbf{a}}_i$和${\mathbf{\omega }}_i$ 是应用于速度，方向和偏置估计的随机脉冲，建模为高斯白噪声过程。它们的均值为零，它们的协方差矩阵由${\mathbf{n}}_a、{\mathbf{n}}_{\mathbf{\omega }}、{\mathbf{n}}_{{\mathbf{b}}_{\mathbf{a}}}$和${\mathbf{n}}_{{\mathbf{b}}_{\mathbf{\omega }}}$的协方差在步长$\Delta t$积分得到(参照)
$$\begin{array}{rl}{\mathbf{V}}_{\mathbf{i}}& ={\sigma }_a^2\Delta t^2\mathbf{I}\\ {\mathbf{\Theta }}_{\mathbf{i}}& ={\sigma }_{\mathbf{\omega }}^2\Delta t^2\mathbf{I}\\ {\mathbf{A}}_{\mathbf{i}}& ={\sigma }_{{\mathbf{b}}_{\mathbf{a}}}^2\Delta t\mathbf{I}\\ {\mathbf{\Omega }}_{\mathbf{i}}& ={\sigma }_{{\mathbf{b}}_{\mathbf{\omega }}}^2\Delta t\mathbf{I}\end{array}$$
其中${\sigma }_a[m/s^2],{\sigma }_{\mathbf{\omega }}[rad/s],{\sigma }_{{\mathbf{b}}_{\mathbf{a}}}[m/s^2\sqrt{s}],{\sigma }_{{\mathbf{b}}_{\mathbf{\omega }}}[rad/s\sqrt{s}]$由IMU的数据手册或标定得到。

The error-state Jacobian and perturbation matrices

x = [ p v q b a b ω g ] δ x = [ δ p δ v δ θ δ b a δ b ω δ g ] u m = [ a m ω m ] i = [ v i θ i a i w i ] \bold{x}=\begin{bmatrix} \bold{p} \\ \bold{v} \\ \bold{q} \\ \bold{b_a} \\ \bold{b}_{\bm{\omega}} \\ \bold{g} \end{bmatrix}\quad \delta \bold{x}=\begin{bmatrix} \delta \bold{p} \\ \delta \bold{v} \\ \delta \bm{\theta} \\ \delta \bold{b_a} \\ \delta \bold{b}_{\bm{\omega}} \\ \delta \bold{g} \end{bmatrix} \quad \bold{u}_m=\begin{bmatrix} \bold{a}_m\\\bm{\omega}_m \end{bmatrix} \quad i=\begin{bmatrix} \bold{v}_{i} \\ \bm{\theta} _{i} \\ \bold{a}_{i} \\ \bold{w}_{i} \end{bmatrix} x= ​pvqba​bω​g​ ​δx= ​δpδvδθδba​δbω​δg​ ​um​=[am​ωm​​]i= ​vi​θi​ai​wi​​ ​
误差状态系统方程可以写作
$$\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}}_i\cdot \mathbf{i}$$
ESKF的预测步公式可以写成：
$$\begin{array}{rl}\widehat{\delta \mathbf{x}}& \leftarrow {\mathbf{F}}_{\mathbf{x}}\left(\mathbf{x},{\mathbf{u}}_m\right)\cdot \widehat{\delta \mathbf{x}}\\ \mathbf{p}& \leftarrow {\mathbf{F}}_{\mathbf{x}}{\mathbf{PF}}_{\mathbf{x}}^T+{\mathbf{F}}_i{\mathbf{Q}}_i{\mathbf{F}}_i^T\end{array}$$
其中$\delta \mathbf{x}\sim \mathcal{N}\{\widehat{\delta \mathbf{x}},\mathbf{p}\}$；${\mathbf{F}}_{\mathbf{x}}$和${\mathbf{F}}_{\mathbf{i}}$是$f()$对于误差和扰动向量的Jacobian矩阵。
以上Jacobian矩阵和协方差矩阵的详细表达式如下，其中所有与状态相关的值都直接从名义状态提取。
F x = ∂ f ∂ δ x ∣ x , u m = [ I I Δ t 0 0 0 0 0 I − R ( a m − b a ) ∧ Δ t − R Δ t 0 I Δ t 0 0 R T { ( ω 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 ] F i = ∂ f ∂ i ∣ x , u m = [ 0 0 0 0 I 0 0 0 0 I 0 0 0 0 I 0 0 0 0 I 0 0 0 0 ] Q i = [ V i 0 0 0 0 Θ i 0 0 0 0 A i 0 0 0 0 Ω i ] \bold{F}_{\bold{x}}= \dfrac{\partial f}{\partial \delta \bold{x}}{\Big\lvert} _{\bold{x},\bold{u}_m}= \begin{bmatrix} \bold{I} & \bold{I}\Delta t & 0 & 0 & 0 & 0 \\ 0 & \bold{I} & -\bold{R} \left( \bold{a}_{m}-\bold{b_a}\right) ^{\wedge }\Delta t & -\bold{R}\Delta t & 0 & \bold{I}\Delta t \\ 0 & 0 & \bold{R}^{T}\left\{ \left( \bm{\omega}_{m}-\bold{b}_{\bm{\omega}}\right) \Delta t\right\} & 0 & -\bold{I}\Delta t & 0 \\ 0 & 0 & 0 & \bold{I} & 0 & 0 \\ 0 & 0 & 0 & 0 & \bold{I} & 0 \\ 0 & 0 & 0 & 0 & 0 & \bold{I} \end{bmatrix} \\ \bold{F}_{i}= \dfrac{\partial f}{\partial \bold{i}}{\Big\lvert}_{\bold{x},\bold{u}_{m}} =\begin{bmatrix} 0 & 0 & 0 & 0 \\ \bold{I} & 0 & 0 & 0 \\ 0 & \bold{I} & 0 & 0 \\ 0 & 0 & \bold{I} & 0 \\ 0 & 0 & 0 & \bold{I} \\ 0 & 0 & 0 & 0 \end{bmatrix}\quad \bold{\bold{Q}}_{i}=\begin{bmatrix} \bold{V_{i}} & 0 & 0 & 0 \\ 0 & \bm{\Theta}_{\bold{i}} & 0 & 0 \\ 0 & 0 & \bold{A_i} & 0 \\ 0 & 0 & 0 & \bm{\Omega}_{\bold{i}} \end{bmatrix} Fx​=∂δx∂f​ ​x,um​​= ​I00000​IΔtI0000​0−R(am​−ba​)∧ΔtRT{(ωm​−bω​)Δt}000​0−RΔt0I00​00−IΔt0I0​0IΔt000I​ ​Fi​=∂i∂f​ ​x,um​​= ​0I0000​00I000​000I00​0000I0​ ​Qi​= ​Vi​000​0Θi​00​00Ai​0​000Ωi​​ ​

Fusing IMU with complementary sensory data

到目前为止，IMU的信息用于对ESKF进行预测，其他信息可以用于滤波器的校正，校正包含三步：

1. 通过观测校正误差状态
2. 将校正后的误差状态加入标称状态
3. 重置误差状态

校正误差状态

通常我们有一个传感器，他的测量取决于状态：
$$\mathbf{y}=h(\mathbf{x},\mathbf{n})$$
其中$\mathbf{n}$是高斯白噪声。
$$\mathbf{n}\sim \mathcal{N}\{0,\mathbf{R}\}$$
滤波器校正步：
$$\begin{array}{rl}\mathbf{K}& ={\mathbf{PH}}^T({\mathbf{HPH}}^T+{\mathbf{MRM}}^T{)}^{-1}\\ \widehat{\delta \mathbf{x}}& \leftarrow \mathbf{K}\left(\mathbf{y}-h(\hat{\mathbf{x}})\right)\\ \mathbf{p}& \leftarrow (\mathbf{I}-\mathbf{KH})\mathbf{p}\end{array}$$
$\mathbf{H}$为相对误差状态$\delta \mathbf{x}$的Jacobian矩阵，且要求在最佳估计状态$\hat{\mathbf{x}}=\bar{\mathbf{x}}⊞\widehat{\delta \mathbf{x}}$处展开。由于在此阶段我们还没有观察到误差状态，故$\hat{\mathbf{x}}=\bar{\mathbf{x}}$，我们可以用名义状态$\bar{\mathbf{x}}$作为评估点。
H ≡ ∂ h ∂ δ x ∣ x ˉ M ≡ ∂ h ∂ n ∣ x ˉ \begin{aligned} \bold{H}&\equiv \dfrac{\partial h}{\partial \delta \bold{x}}{\Big \lvert}_{\bar{\bold{x}}} \\ \bold{M}&\equiv\dfrac{\partial h}{\partial \bold{n}}{\Big \lvert}_{\bar{\bold{x}}}\\ \end{aligned} HM​≡∂δx∂h​ ​xˉ​≡∂n∂h​ ​xˉ​​

Jacobian矩阵的计算

H ≜ ∂ h ∂ δ x ∣ x ˉ = ∂ h ∂ x ∣ x ˉ ∂ x ∂ δ x ∣ x ˉ = H x X δ x \bold{H}\triangleq \dfrac{\partial h}{\partial \delta \bold{x}}{\Big\lvert}_{\bar{\bold{x}}}=\dfrac{\partial h}{\partial \bold{x}}{\Big\lvert}_{\bar{\bold{x}}}\dfrac{\partial \bold{x}}{\partial \delta \bold{x}}{\Big\lvert}_{\bar{\bold{x}}}=\bold{H_x} \bold{X}_{\delta \bold{x}} H≜∂δx∂h​ ​xˉ​=∂x∂h​ ​xˉ​∂δx∂x​ ​xˉ​=Hx​Xδx​
其中 H x ≜ ∂ h ∂ x ∣ x ˉ \bold{H_x}\triangleq \dfrac{\partial h}{\partial \bold{x}}{\Big\lvert}_{\bar{\bold{x}}} Hx​≜∂x∂h​ ​xˉ​是通常卡尔曼滤波中$h()$的Jacobian矩阵。
X δ x = [ ∂ ( p + δ p ) ∂ δ p ∂ ( v + δ v ) ∂ δ v 0 ∂ ( q ⊗ δ q ) ∂ δ θ ∂ ( b a + δ b a ) ∂ δ b a 0 ∂ ( b ω + δ b ω ) ∂ δ b ω ∂ ( g + δ g ) ∂ δ g ] = [ I 6 0 0 0 Q δ θ 0 0 0 I 9 ] \begin{aligned} \bold{X}_{\delta \bold{x}}&= \begin{bmatrix} \dfrac{\partial (\bold{p}+\delta \bold{p})}{\partial \delta \bold{p}}&&&&&\\ & \dfrac{\partial (\bold{v}+\delta \bold{v})}{\partial \delta \bold{v}}&&&0&\\ &&\dfrac{\partial (\bold{q}\otimes\delta \bold{q})}{\partial \delta \bm{\theta}}&&&\\ &&&\dfrac{\partial (\bold{b_a}+\delta \bold{b_a})}{\partial \delta \bold{b_a}}&&\\ &0&&&\dfrac{\partial (\bold{b}_{\bm{\omega}}+\delta \bold{b}_{\bm{\omega}})}{\partial \delta \bold{b}_{\bm{\omega}}}&\\ &&&&&\dfrac{\partial (\bold{g}+\delta \bold{g})}{\partial \delta \bold{g}} \end{bmatrix}\\ &=\begin{bmatrix} \bold{I}_6&0&0\\ 0 & \bold{Q}_{\delta \bm{\theta}}&0\\ 0&0&\bold{I}_9 \end{bmatrix} \end{aligned} Xδx​​= ​∂δp∂(p+δp)​​∂δv∂(v+δv)​0​∂δθ∂(q⊗δq)​​∂δba​∂(ba​+δba​)​​0∂δbω​∂(bω​+δbω​)​​∂δg∂(g+δg)​​ ​= ​I6​00​0Qδθ​0​00I9​​ ​​
Q δ θ = ∂ ( q ⊗ δ q ) ∂ δ θ ∣ q = ∂ ( q ⊗ δ q ) ∂ δ q ∣ q ∂ δ q ∂ δ θ ∣ δ θ ^ = 0 = ∂ ( [ q ] L δ q ) ∂ δ q ∣ q ∂ [ 1 1 2 δ θ ] ∂ δ θ ∣ δ θ ^ = 0 = [ q ] L 1 2 [ 0 0 0 1 0 0 0 1 0 0 0 1 ] = 1 2 [ − q x − q y − q z q w − q z q y q z q w − q x − q y q x q w ] \begin{aligned} \bold{Q}_{\delta \bm{\theta}}&=\dfrac{\partial(\bold{q}\otimes \delta \bold{q})}{\partial \delta \bm{\theta}}{\Big\lvert}_{\bold{q}}=\dfrac{\partial(\bold{q}\otimes \delta \bold{q})}{\partial\delta \bold{q}}{\Big\lvert}_q \dfrac{\partial\delta \bold{q}}{\partial \delta \bm{\theta}}{\Big\lvert}_{\hat{\delta \bm{\theta}}=0}\\ &=\dfrac{\partial([\bold{q}]_L \delta \bold{q})}{\partial\delta \bold{q}}{\Big\lvert}_q \dfrac{\partial\begin{bmatrix}1\\\frac{1}{2}\delta \bm{\theta}\end{bmatrix}}{\partial \delta \bm{\theta}}{\Big\lvert}_{\hat{\delta \bm{\theta}}=0} \\&=[\bold{q}]_L \dfrac{1}{2}\begin{bmatrix}0&0&0\\1&0&0\\0&1&0\\0&0&1\end{bmatrix} \\&=\dfrac{1}{2}\begin{bmatrix}-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{bmatrix} \end{aligned} Qδθ​​=∂δθ∂(q⊗δq)​ ​q​=∂δq∂(q⊗δq)​ ​q​∂δθ∂δq​ ​δθ^=0​=∂δq∂([q]L​δq)​ ​q​∂δθ∂[121​δθ​]​ ​δθ^=0​=[q]L​21​ ​0100​0010​0001​ ​=21​ ​−qx​qw​qz​−qy​​−qy​−qz​qw​qx​​−qz​qy​−qx​qw​​ ​​

校正误差加入名义状态

在ESKF更新之后，就需要用校正后的误差状态去校正名义状态
$$\mathbf{x}\leftarrow \mathbf{x}⊞\widehat{\delta \mathbf{x}}$$
即
$$\begin{array}{rl}\mathbf{p}& \leftarrow \mathbf{p}+\widehat{\delta \mathbf{p}}\\ \mathbf{v}& \leftarrow \mathbf{v}+\widehat{\delta \mathbf{v}}\\ \mathbf{q}& \leftarrow \mathbf{q}\otimes \mathbf{q}\{\widehat{\delta \mathbf{\theta }}\}\\ {\mathbf{b}}_a& \leftarrow {\mathbf{b}}_{\mathbf{a}}+\widehat{\delta {\mathbf{b}}_{\mathbf{a}}}\\ {\mathbf{b}}_{\mathbf{\omega }}& \leftarrow {\mathbf{b}}_{\mathbf{\omega }}+\widehat{\delta {\mathbf{b}}_{\mathbf{\omega }}}\\ \mathbf{g}& \leftarrow \mathbf{g}+\widehat{\delta \mathbf{g}}\end{array}$$

ESKF reset

在误差状态加入名义状态后，误差状态均值$\widehat{\delta \mathbf{x}}$被重置。这与方向部分尤其相关，因为新的方向误差在新的名义状态的局部坐标系表达。为了使ESKF更新完整，需要根据此修改误差状态的协方差。
令误差状态重置函数为$\mathbf{g}(\cdot )$
$$\delta \mathbf{x}\leftarrow \mathbf{g}(\delta \mathbf{x})=\delta \mathbf{x}\boxminus \widehat{\delta \mathbf{x}}$$
$$\begin{array}{rl}\widehat{\delta \mathbf{x}}& \leftarrow 0\\ \mathbf{p}& \leftarrow {\mathbf{GPG}}^T\end{array}$$
其中$\mathbf{G}$是Jacobian矩阵
G ≜ ∂ g ∂ δ x ∣ δ x ^ = [ I 6 0 0 0 I − ( 1 2 δ θ ^ ) ∧ 0 0 0 I 9 ] \begin{aligned} \bold{G}&\triangleq\dfrac{\partial \bold{g}}{\partial \delta \bold{x}}{\Big \lvert}_{\hat{\delta \bold{x}}}\\ &=\begin{bmatrix} I_6&0&0\\ 0&\bold{I}-\left(\frac{1}{2}\hat{\delta\bm{\theta}}\right)^{\wedge}&0\\ 0&0&I_9 \end{bmatrix} \end{aligned} G​≜∂δx∂g​ ​δx^​= ​I6​00​0I−(21​δθ^)∧0​00I9​​ ​​
下面给出方向误差块的导数推导

Jacobian of the reset operation to the orientation error

基于以下事实：

• 真实的方向经过error reset是不改变的，即${\mathbf{q}}^{+}=\mathbf{q}$
  $${\bar{\mathbf{q}}}^{+}\otimes \delta {\mathbf{q}}^{+}=\bar{\mathbf{q}}\otimes \delta \mathbf{q}$$
• 观测误差的均值已经被加入名义状态
  $${\bar{\mathbf{q}}}^{+}=\bar{\mathbf{q}}\otimes \widehat{\delta \mathbf{q}}$$
  将两个事实结合起来，可得
  $$\delta {\mathbf{q}}^{+}=({\bar{\mathbf{q}}}^{+}{)}^{\ast }\otimes \bar{\mathbf{q}}\otimes \delta \mathbf{q}={\left(\bar{\mathbf{q}}\otimes \widehat{\delta \mathbf{q}}\right)}^{\ast }\otimes \bar{\mathbf{q}}\otimes \delta \mathbf{q}={\widehat{\delta \mathbf{q}}}^{\ast }\otimes \delta \mathbf{q}=[{\widehat{\delta \mathbf{q}}}^{\ast }{]}_L\cdot \delta \mathbf{q}$$
  使用四元数小量近似${\widehat{\delta \mathbf{q}}}^{\ast }\approx \left[\begin{array}{c}1\\ -\frac{1}{2}\widehat{\delta \mathbf{\theta }}\end{array}\right]$，上式可化为
  [ 1 1 2 δ θ + ] = [ 1 1 2 δ θ ^ T − 1 2 δ θ ^ I − ( 1 2 δ θ ^ ) ∧ ] [ 1 1 2 δ θ ] + O ( ∥ δ θ ∥ 2 ) \begin{bmatrix}1\\\frac{1}{2}\delta \bm{\theta}^{+}\end{bmatrix}=\begin{bmatrix}1&\frac{1}{2}\hat{\delta \bm{\theta}}^T\\-\frac{1}{2}\hat{\delta \bm{\theta}}&\bold{I}-\left(\frac{1}{2}\hat{\delta\bm{\theta}}\right)^{\wedge}\end{bmatrix} \begin{bmatrix}1\\\frac{1}{2}\delta \bm{\theta}\end{bmatrix}+O(\lVert\delta \bm{\theta}\rVert^2) [121​δθ+​]= ​1−21​δθ^​21​δθ^TI−(21​δθ^)∧​ ​[121​δθ​]+O(∥δθ∥2)
  得到一个标量和一个矢量等式
  $$\begin{array}{rl}\frac{1}{4}{\widehat{\delta \mathbf{\theta }}}^T\delta \mathbf{\theta }& =O(\Vert \delta \mathbf{\theta }{\Vert }^2)\\ \delta {\mathbf{\theta }}^{+}& =-\widehat{\delta \mathbf{\theta }}+\left(\mathbf{I}-{\left(\frac{1}{2}\widehat{\delta \mathbf{\theta }}\right)}^{\wedge }\right)\delta \mathbf{\theta }+O(\Vert \delta \mathbf{\theta }{\Vert }^2)\end{array}$$
  第一个式子信息量不大，从第二个式子中我们可以看出${\widehat{\delta \mathbf{\theta }}}^{+}=0$，这是我们在重置操作中期望得到的。
  $$\frac{\partial \delta {\mathbf{\theta }}^{+}}{\partial \delta \mathbf{\theta }}=\mathbf{I}-{\left(\frac{1}{2}\widehat{\delta \mathbf{\theta }}\right)}^{\wedge }$$

附录

Integration of the angular error

考虑无偏差和噪声的角误差动力学
$$\delta \mathbf{\theta }=-{\mathbf{\omega }}^{\wedge }\delta \mathbf{\theta }$$
状态转移矩阵可以写成泰勒级数的形式：
$$\begin{array}{rl}\mathbf{\varphi }& =e^{-{\mathbf{\omega }}^{\wedge }\Delta t}\\ & =\mathbf{I}-{\mathbf{\omega }}^{\wedge }\Delta t+\frac{1}{2!}{\mathbf{\omega }}^{\wedge 2}\Delta t^2-\frac{1}{3!}{\mathbf{\omega }}^{\wedge 3}\Delta t^3+\frac{1}{4!}{\mathbf{\omega }}^{\wedge 4}\Delta t^4-\dots \end{array}$$
定义$\mathbf{\omega }\Delta t=\mathbf{u}\Delta \mathbf{\theta }$为轴角的形式
$$\begin{array}{rl}\mathbf{\varphi }& =\mathbf{I}-{\mathbf{u}}^{\wedge }\left(\Delta \mathbf{\theta }-\frac{\Delta {\mathbf{\theta }}^3}{3!}+\frac{\Delta {\mathbf{\theta }}^5}{5!}-\dots \right)+{\mathbf{u}}^{\wedge 2}\left(\frac{\Delta {\mathbf{\theta }}^2}{2!}-\frac{\Delta {\mathbf{\theta }}^4}{4!}+\frac{\Delta {\mathbf{\theta }}^6}{6!}-\dots \right)\\ & =\mathbf{I}-{\mathbf{u}}^{\wedge }\sin \Delta \mathbf{\theta }+{\mathbf{u}}^{\wedge 2}\left(\mathbf{I}-\cos \Delta \mathbf{\theta }\right)\end{array}$$
根据罗德里格斯公式，状态转移矩阵对应
$$\mathbf{\varphi }=\mathbf{R}\left\{-\mathbf{u}\Delta \mathbf{\theta }\right\}=\mathbf{R}{\left\{\mathbf{\omega }t\right\}}^T$$

Integration of random noise and perturbations

我们的目标是在动态系统中给出适当的随机变量积分方法。当然，我们不能积分未知随机值，但我们可以积分它们的方差和协方差，以实现不确定性的传播。这是为了在具有连续性质(在连续时间中指定)但以离散方式估计的系统的估计器中建立协方差矩阵所需要的。
考虑连续时间动态系统，
$$\dot{\mathbf{x}}=f\left(\mathbf{x},\mathbf{u},\mathbf{w}\right)$$
其中$\mathbf{x}$是状态向量，$\mathbf{u}$是包含噪声$\tilde{\mathbf{u}}$的控制信号向量，因此控制测量值为${\mathbf{u}}_m=\mathbf{u}+\tilde{\mathbf{u}}$，$\mathbf{w}$是随机扰动向量。噪声和扰动都假设为白高斯噪声：
$$\tilde{\mathbf{u}}\sim \mathcal{N}\left\{0,{\mathbf{U}}^c\right\}\mathbf{w}\sim \mathcal{N}\left\{0,{\mathbf{W}}^c\right\}$$
其中上标${\cdot }^c$表示连续时间。
控制信号的噪声$\tilde{\mathbf{u}}$和随机扰动$\mathbf{w}$之间存在本质的区别：

• 在离散化过程中，控制信号在$\mathbf{n}\Delta t$瞬间采样。在积分区间内，测量部分显然被认为是恒定的，即$${\mathbf{u}}_m(t)={\mathbf{u}}_m(\mathbf{n}\Delta t)=\mathbf{u}(\mathbf{n}\Delta t)+\tilde{\mathbf{u}}(\mathbf{n}\Delta t)\mathbf{n}\Delta t<t<(\mathbf{n}+1)\Delta t$$噪声也同样保持不变$$\tilde{\mathbf{u}}(t)=\tilde{\mathbf{u}}(\mathbf{n}\Delta t)={\tilde{\mathbf{u}}}_n\mathbf{n}\Delta t<t<(\mathbf{n}+1)\Delta t$$
• 扰动$\mathbf{w}$是从未采样过的。

因此，两种随机过程在$\Delta t$间隔内的积分也不同。

误差状态动态方程为
$$\dot{\delta \mathbf{x}}=\mathbf{A}\delta \mathbf{x}+\mathbf{B}\tilde{\mathbf{u}}+\mathbf{Cw}$$
其中
A ≜ ∂ f ∂ δ x ∣ x , u m B ≜ ∂ f ∂ u ~ ∣ x , u m C ≜ ∂ f ∂ w ∣ x , u m \bold{A}\triangleq \dfrac{\partial f}{\partial \delta \bold{x}}{\Big\lvert} _{\bold{x},\bold{u}_{m}}\qquad \bold{B}\triangleq \dfrac{\partial f}{\partial \tilde{\bold{u}}}{\Big\lvert}_{ \bold{x},\bold{u}_{m}}\qquad \bold{C}\triangleq \dfrac{\partial f}{\partial \bold{w}}{\Big\lvert}_{\bold{x},\bold{u}_{m}} A≜∂δx∂f​ ​x,um​​B≜∂u~∂f​ ​x,um​​C≜∂w∂f​ ​x,um​​
经过采样时间$\Delta t$的积分，有
$$\begin{array}{rl}\delta {\mathbf{x}}_{n+1}& =\delta {\mathbf{x}}_n+{\int }_{n\Delta t}^{\left(n+1\right)\Delta t}\left[\mathbf{A}\delta \mathbf{x}\left(\tau \right)+\mathbf{B}\tilde{\mathbf{u}}\left(\tau \right)+\mathbf{Cw}\left(\tau \right)\right]d\tau \\ & =\delta {\mathbf{x}}_{\mathbf{n}}+{\int }_{n\Delta t}^{\left(n+1\right)\Delta t}\mathbf{A}\delta \mathbf{x}\left(\tau \right)d\tau +{\int }_{n\Delta t}^{\left(n+1\right)\Delta t}\mathbf{B}\tilde{\mathbf{u}}\left(\tau \right)d\tau +{\int }_{n\Delta t}^{\left(n+1\right)\Delta t}\mathbf{Cw}\left(\tau \right)d\tau \end{array}$$
式中有三项不同本质的积分。

1. 第一项状态向量部分的积分
   $$\delta {\mathbf{x}}_{\mathbf{n}}+{\int }_{\mathbf{n}\Delta t}^{\left(\mathbf{n}+1\right)\Delta t}\mathbf{A}\delta \mathbf{x}\left(\tau \right)\mathrm{d}\tau =\mathbf{\Phi }\cdot \delta {\mathbf{x}}_{\mathbf{n}}$$
   其中$\mathbf{\Phi }=e^{\mathbf{A}\Delta t}$可以求出闭式解
2. 第二项控制向量的噪声部分的积分
   $${\int }_{\mathbf{n}\Delta t}^{\left(\mathbf{n}+1\right)\Delta t}\mathbf{B}\tilde{\mathbf{u}}\left(\tau \right)d\tau =\mathbf{B}\Delta t{\tilde{\mathbf{u}}}_{\mathbf{n}}$$
   这意味着一旦采样测量噪声以确定的方式积分，因为它在积分区间内的行为是已知的。
3. 由概率论可知，连续高斯白噪声在周期$\Delta t$上的积分产生一个离散高斯白脉冲$w_n$，描述为
   $${\mathbf{w}}_{\mathbf{n}}\triangleq {\int }_{\mathbf{n}\Delta t}^{\left(\mathbf{n}+1\right)\Delta t}\mathbf{w}\left(\tau \right)d\tau {\mathbf{w}}_{\mathbf{n}}\sim \mathcal{N}\left\{0,\mathbf{W}\right\}\text{with }\mathbf{W}={\mathbf{W}}^c\Delta t$$
   我们观察到，与上面的测量噪声相反，扰动在积分区间内没有确定的行为，因此它必须是随机积分的。
   $$\delta {\mathbf{x}}_{\mathbf{n}+1}={\mathbf{F}}_{\mathbf{x}}\delta {\mathbf{x}}_{\mathbf{n}}+{\mathbf{F}}_{\mathbf{n}}{\tilde{\mathbf{u}}}_{\mathbf{n}}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_{\mathbf{n}}$$
   转换矩阵、控制矩阵和扰动矩阵为
   $${\mathbf{F}}_{\mathbf{x}}=\mathbf{\Phi }=e^{\mathbf{A}\Delta t},{\mathbf{F}}_{\mathbf{u}}=\mathbf{B}\Delta t,{\mathbf{F}}_{\mathbf{w}}=\mathbf{C}$$
   噪声和扰动的定义为
   $${\mathbf{u}}_n\sim \mathcal{N}\{0,\mathbf{U}\},{\mathbf{w}}_n\sim \mathcal{N}\{0,\mathbf{W}\}$$
   其中$\mathbf{U}={\mathbf{U}}^c,\mathbf{W}={\mathbf{W}}^c\Delta t$
   EKF的预测阶段将计算误差状态的均值和协方差矩阵
   $$\begin{array}{rl}{\check{\delta \mathbf{x}}}_{\mathbf{n}+1}& ={\mathbf{F}}_{\mathbf{x}}{\widehat{\delta \mathbf{x}}}_n\\ {\check{\mathbf{p}}}_{\mathbf{n}+1}& ={\mathbf{F}}_{\mathbf{x}}{\hat{\mathbf{p}}}_n{{\mathbf{F}}_{\mathbf{x}}}^T+{\mathbf{F}}_{\mathbf{u}}\mathbf{U}{{\mathbf{F}}_{\mathbf{u}}}^T+{\mathbf{F}}_{\mathbf{w}}\mathbf{W}{{\mathbf{F}}_{\mathbf{w}}}^T\\ & =e^{\mathbf{A}\Delta t}{\hat{\mathbf{p}}}_n{\left(e^{\mathbf{A}\Delta t}\right)}^T+\Delta t^2\mathbf{B}{\mathbf{U}}^c{\mathbf{B}}^T+\Delta t{\mathbf{CW}}^c{\mathbf{C}}^T\end{array}$$
   在这里，观察积分区间$\Delta t$对协方差更新的三个项的不同影响是非常重要的，也是具有说明意义的:动态误差项是指数项，测量误差项是二次项，扰动误差项是线性项。