扩展卡尔曼滤波器

概述

本文对上文的卡尔曼滤波知识进行些许补充，并简单说明扩展卡尔曼的知识及用法。

正文

对卡尔曼滤波的补充

卡尔曼是一种多数据递归融合算法，同时也可以理解为一个权重在变化的低通滤波器。

其思想公式为：

$当前的估计值=上一次估计值+权重\ast （当前测量值-上一次估计值）$

所以，递归次数越多，测量值的作用越小，其估计值越接近真实值。如下例子：

假设A的真实值为50，经过上述公式计算后，结果波形如下：

所谓的数据融合，其实就是通过计算增益值K使得测量值的方差$\Sigma$最小，使得数据为最优解。

使用卡尔曼滤波器的步骤：

(1)建立系统的状态空间方程和测量方程

例如：二维的位置P、速度V恒定速度系统，测量值为P，无控制量

$P_k=P_{k-1}+V_k\ast \Delta t+W_{p,k}$

$V_k=V_{k-1}+W_{v,k}$

$Z_{p,k}=X_{p,k}+V_{p,k}$

$Z_{v,k}=0$

$W$、$V$为过程噪声和测量噪声。

设状态向量 X k ^ = ∣ P k V k ∣ T \hat{X_k} = |PkVk|

^T Xk​^​=∣ ∣​Pk​​Vk​​∣ ∣​T

所以用矩阵表示为：

∣ P k V k ∣ = ∣ 1 Δ t 0 1 ∣ ∣ P k − 1 V k − 1 ∣ + ∣ Q p 0 0 Q v ∣ ∣ W p , k W v , k ∣ |PkVk|

=|1Δt01||Pk−1Vk−1|+|Qp00Qv||Wp,kWv,k| ∣ ∣​Pk​Vk​​∣ ∣​=∣ ∣​10​Δt1​∣ ∣​∣ ∣​Pk−1​Vk−1​​∣ ∣​+∣ ∣​Qp​0​0Qv​​∣ ∣​∣ ∣​Wp,k​Wv,k​​∣ ∣​

Z k = ∣ 1 0 ∣ ∣ X p , k − 1 X v , k − 1 ∣ + ∣ R p 0 ∣ ∣ V p , k − 1 V v , k − 1 ∣ Z_k=|10|

|Xp,k−1Xv,k−1|+|Rp0||Vp,k−1Vv,k−1| Zk​=∣ ∣​1​0​∣ ∣​∣ ∣​Xp,k−1​Xv,k−1​​∣ ∣​+∣ ∣​Rp​​0​∣ ∣​∣ ∣​Vp,k−1​Vv,k−1​​∣ ∣​

(2)将矩阵代入公式计算

这里状态转移矩阵 F = ∣ 1 Δ t 0 1 ∣ F=|1Δt01|

F=∣ ∣​10​Δt1​∣ ∣​

由于没有控制量，所以$B=0$

过程噪声协方差矩阵$Q$其实就是前面的 ∣ Q p 0 0 Q v ∣ |Qp00Qv|

∣ ∣​Qp​0​0Qv​​∣ ∣​

测量噪声协方差矩阵$R$其实就是前面的 ∣ R p 0 ∣ |Rp0|

∣ ∣​Rp​​0​∣ ∣​

由于状态协方差矩阵只在初始阶段产生影响，且位置速度噪声相互独立，所以一般可以给定矩阵初始值 P 0 = ∣ σ p 2 σ p σ v σ v σ p σ v 2 ∣ = ∣ 1 0 0 1 ∣ P_0=|σp2σpσvσvσpσv2|

=|1001| P0​=∣ ∣​σp​2σv​σp​σp​σv​σv​2​∣ ∣​=∣ ∣​10​01​∣ ∣​

系统转换矩阵 H = ∣ 1 0 ∣ H=|10|

H=∣ ∣​1​0​∣ ∣​

扩展卡尔曼滤波器

扩展卡尔曼应用于非线性程度不高的系统。扩展卡尔曼其实与卡尔曼很类似，只是扩展卡尔曼是利用泰勒级数展开对$k-1$时刻求$f(x)$对$x$的偏导。所以两者最大的区别是状态转移矩阵$F$和系统转换矩阵$H$用雅各比矩阵表示。

即扩展卡尔曼先验估算公式为：

${\hat{X}}_k=f({\hat{x}}_{k-1},{\hat{u}}_{k-1},0)$

后验估算公式为：

${\hat{X}}_k^{\prime }={\hat{X}}_k+K(Z_k-h({\hat{X}}_k))$

例如：

某系统的状态空间方程为：

$X_1=X_1+SIN(X_2)=f_1(x)$

$X_2=X_1^2=f_2(x)$

$Z_1=\frac{1}{2}COS(X_1)=h_1(x)$

$Z_2=0=h_2(x)$

所以：

F = ∣ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ∣ x 1 k − 1 , x 2 k − 1 = ∣ 1 c o s ( x 2 k − 1 ) 2 x 1 k − 1 0 ∣ F=|∂f1∂x1∂f1∂x2∂f2∂x1∂f2∂x2|

_{x_{1k-1},x_{2k-1}}=|1cos(x2k−1)2x1k−10| F=∣ ∣​∂x1​∂f1​​∂x1​∂f2​​​∂x2​∂f1​​∂x2​∂f2​​​∣ ∣​x1k−1​,x2k−1​​=∣ ∣​12x1k−1​​cos(x2k−1​)0​∣ ∣​

H = ∣ ∂ h 1 ∂ x 1 ∂ h 1 ∂ x 2 ∂ h 2 ∂ x 1 ∂ h 2 ∂ x 2 ∣ x 1 k − 1 , x 2 k − 1 = ∣ − 1 2 s i n ( x 1 k − 1 ) 0 ∣ H=|∂h1∂x1∂h1∂x2∂h2∂x1∂h2∂x2|

_{x_{1k-1},x_{2k-1}}=|−12sin(x1k−1)0| H=∣ ∣​∂x1​∂h1​​∂x1​∂h2​​​∂x2​∂h1​​∂x2​∂h2​​​∣ ∣​x1k−1​,x2k−1​​=∣ ∣​−21​sin(x1k−1​)​0​∣ ∣​

由此可以看出$F$、$H$随着$k$的变化而变化。

其余公式与卡尔曼滤波公式一致，将这两个雅各比矩阵代入重新计算即可。

总结

如上所说，即可完成将卡尔曼滤波器改写成扩展卡尔曼滤波器。