扩展卡尔曼滤波在目标跟踪中的应用(1)

前面几节相继介绍的不同维度的卡尔曼滤波算法在线性高斯模型的条件下,具有很好的跟踪效果,当时在实际生活中,不可能所有的运动都呈现一种线性关系,也存在着非线性关系,比如说:飞机的飞行状态,导弹的制导问题等等。为了保证结果的准确性和精确度,因此就需要有一种适用于非线性系统的滤波算法
对于非线性系统的滤波问题将非线性系统转换为一个近似线性的系统然后进行滤波是一种较为常用的方法,例如本节将介绍的扩展卡尔曼滤波EKF算法

EKF实现思路

扩展Kalman滤波建立在线性Kalman滤波算法上,对一般的非线性系统,主要思路为:

  • 围绕滤波值 X ^ k \hat{X}_{k} X^k非线性函数f(*)和h(*)展开成泰勒(Taylor)级数略去二阶及以上项
  • 然后得到一个近似的线性化模型,然后应用Kalman滤波完成对自标的滤波估计等处理。

EKF滤波原理

在之前的线性Kalman滤波中,状态方程和量测方程如下所示:
X k = F X k − 1 + V k ( 1 ) X_k =FX_{k-1}+V_{k} \quad(1) Xk=FXk1+Vk(1)
Z k = H X k + W k ( 1 ) Z_k =HX_{k}+W_{k} \quad(1) Zk=HXk+Wk(1)

由于此时的非线性模型,因此状态转移不再是严格的线性关系,因此,上述的两个方程变为了下式:
X k = f ( k , X k − 1 ) + V k ( 3 ) X_k =f(k,X_{k-1})+V_{k} \quad(3) Xk=f(k,Xk1)+Vk(3)
Z k = h ( k , X k ) + W k ( 4 ) Z_k =h(k,X_k)+W_{k} \quad(4) Zk=h(k,Xk)+Wk(4)
按照EKF的思路,首先进行局部的线性化

系统状态方程的局部线性化

非线性函数$f(k,X_{k-1})$围绕滤波值$\hat X_k$进行一阶展开如下:
X ( k + 1 ) ≈ f [ k , X ^ ( k ) ] + ∂ f ∂ X ^ ( k ) [ X ( k ) − X ^ ( k ) ] + V k ( 5 ) X(k+1)\approx f[k,\hat{X}(k)]+\frac{\partial f}{\partial\hat{X}(k)}[X(k)-\hat{X}(k)]+V_k \quad(5) X(k+1)f[k,X^(k)]+X^(k)f[X(k)X^(k)]+Vk(5)
令其中的:
∂ f ∂ X ^ ( k ) = ∂ f [ X ^ ( k ) , k ] ∂ X ^ ( k ) ∣ X ^ ( k ) = X ( k ) = Φ ( k + 1 ∣ k ) ( 6 ) \left.\frac{\partial f}{\partial\hat{X}\left(k\right)}=\frac{\partial f\left[\hat {{X}}(k),k\right]}{\partial\hat{X}\left(k\right)}\right|_{\hat{X}\left(k\right)=X\left(k\right)}=\Phi(k+1|k)\quad(6) X^(k)f=X^(k)f[X^(k),k] X^(k)=X(k)=Φ(k+1∣k)(6)
f [ k , X ^ ( k ) ] − ∂ f ∂ X ( k ) ∣ X ( k ) = X ^ ( k ) X ^ ( k ) = ϕ ( k ) ( 7 ) f\left[k,\hat{X}(k)\right]-\left.\frac{\partial f}{\partial X(k)}\right|_{X(k)=\hat{X}(k)}\hat{X}(k)=\phi(k)\quad(7) f[k,X^(k)]X(k)f X(k)=X^(k)X^(k)=ϕ(k)(7)
此时上述的(6)式就可以看作是非线性模型下的状态转移方程,此时的状态方程可以写为:
X ( k + 1 ) = Φ ( k + 1 ∣ k ) X ( k ) + V k + ϕ ( k ) ( 8 ) X\left(k+1\right)=\Phi\left(k+1|k\right)X\left(k\right)+V_k+\phi\left(k\right)\quad(8) X(k+1)=Φ(k+1∣k)X(k)+Vk+ϕ(k)(8)
可以看到,相对基本的Kalman滤波方式中的状态方程,此方程中多了一项,该项为非随机外作用项

量测方程的局部线性化

采用同样的方法对量测方程进行局部线性化,如下:
Z ( k ) = h [ X ^ ( k ∣ k − 1 ) , k ] + ∂ h ∂ X ^ ( k ) ∣ X ^ ( k , k − 1 ) [ X ( k ) − X ^ ( k ∣ k − 1 ) ] + W k ( 9 ) Z(k)=h[\hat{X}(k\mid k-1),k]+\left.\frac{\partial h}{\partial\hat{X}(k)}\right|_{\hat{X}(k,k-1)}[X(k)-\hat{X}(k\mid k-1)]+W_k\quad(9) Z(k)=h[X^(kk1),k]+X^(k)h X^(k,k1)[X(k)X^(kk1)]+Wk(9)
令其中的:
∂ h ∂ X ^ ( k ) ∣ X ( k ) = X ^ ( k ) = H ( k ) ( 10 ) \left.\frac{\partial h}{\partial\hat{X}\left(k\right)}\right|_{X\left(k\right)=\hat{X}\left(k\right)}=H\left(k\right)\quad(10) X^(k)h X(k)=X^(k)=H(k)(10)
y ( k ) = h [ X ^ ( k ∣ k − 1 ) , k ] − ∂ h ∂ X ^ ( k ) ∣ X ( k ) = X ^ ( k ) X ^ ( k ∣ k − 1 ) ( 11 ) y(k)=h[\hat{X}(k|k-1),k]-\frac{\partial h}{\partial\hat{X}(k)|_{X(k)=\hat{X}(k)}}\hat{X}(k|k-1)\quad(11) y(k)=h[X^(kk1),k]X^(k)X(k)=X^(k)hX^(kk1)(11)
同样整理得到非线性模型下量测方程如下:
Z ( k ) = H ( k ) X ( k ) + y ( k ) + W k ( 12 ) Z\left(k\right)=H\left(k\right)X\left(k\right)+y\left(k\right)+W_k\quad(12) Z(k)=H(k)X(k)+y(k)+Wk(12)

EKF滤波

经过了局部的初始化后,即可以按照卡尔曼滤波的基本共识与流程进行滤波,基本公式如下:
在这里插入图片描述
滤波的初值和初始协方差矩阵为:
在这里插入图片描述

状态转移矩阵与量测矩阵的计算

线性化后的系统方程中,状态转移矩阵 Φ ( k + 1 ∣ k ) \Phi(k+1|k) Φ(k+1∣k)量测矩阵 H ( k + 1 ) H(k+1) H(k+1)可以通过f和h的雅各比矩阵代替,假设状态变量X有n维,也即
X = [ x 1 x 2 ⋯ x n ] T X = \left[ \begin{array} { l l l l } { x _ { 1 } } & { x _ { 2 } } & { \cdots } & { x _ { n } } \end{array} \right]^T X=[x1x2xn]T
对应的计算公式分别如下:
在这里插入图片描述
在这里插入图片描述
明日将会这部分内容进行仿真测试,上述内容即使今天的全部内容了,感谢大家的观看。

如果方便,辛苦大家点个赞和关注哦!
您的点赞或评论或关注是对我最大的肯定,谢谢大家!!!

  • 1
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

机器 xiaohu

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值