扩展卡尔曼滤波EKF—目标跟踪中的应用(算法部分)

扩展卡尔曼滤波EKF—目标跟踪中的应用(算法部分)

原创不易,路过的各位大佬请点个赞

仿真部分见博客:
扩展卡尔曼滤波EKF—目标跟踪中的应用(仿真部分)

https://blog.csdn.net/weixin_44044161/article/details/115329181             

作者:[email protected]
备注:
扩展卡尔曼滤波算法;
目标跟踪matlab仿真实现;
Case: 二维目标跟踪情况和三维目标跟踪情况
代码下载地址如下(分别为二维情形和三维情形)

EKF仿真代码:二维目标跟踪

https://download.csdn.net/download/weixin_44044161/16239356 

EKF仿真代码:三维目标跟踪

https://download.csdn.net/download/weixin_44044161/16239480            

扩展卡尔曼滤波—及其在目标跟踪中的应用

  • 扩展卡尔曼滤波EKF—目标跟踪中的应用(算法部分)
    • 一、带加性噪声的扩展卡尔曼滤波算法
      • 1.1 问题描述(离散时间非线性系统描述)
      • 1.2 扩展卡尔曼滤波器(EKF)
    • 二、带非加性噪声的扩展卡尔曼滤波算法
      • 2.1 问题描述(离散时间非线性系统描述)
      • 2.2 带非加性噪声的扩展卡尔曼滤波器(EKF)
    • 三、仿真实验
      • 3.1 仿真场景(三维)
      • 3.2 仿真场景(二维)
      • 3.3 二维EKF跟踪仿真结果
    • 四、二维EKF跟踪参考代码
      • 4.1 主函数
      • 4.2 EKF 滤波器函数
      • 4.3 量测方程函数

一、带加性噪声的扩展卡尔曼滤波算法

1.1 问题描述(离散时间非线性系统描述)

考虑带加性噪声的一般非线性系统模型,
x k = f ( x k − 1 ) + w k − 1 z k = h ( x k ) + v k (1) x_k=f(x_{k-1}) +w_{k-1} \\ z_k=h(x_k)+v_k \tag{1} xk=f(xk1)+wk1zk=h(xk)+vk(1)
其中 x k x_k xk k k k时刻的目标状态向量。 z k z_k zk k k k时刻量测向量(传感器数据)。这里不考虑控制器 u k u_k uk w k {w_k} wk v k {v_k} vk分别是过程噪声序列和量测噪声序列,并假设 w k w_k wk v k v_k vk为零均值高斯白噪声,其方差分别为 Q k Q_k Qk R k R_k Rk的高斯白噪声,即 w k ∼ ( 0 , Q k ) w_k\sim(0,Q_k) wk(0,Qk), v k ∼ ( 0 , R k ) v_k\sim(0,R_k) vk(0,Rk),且满足如下关系(线性高斯假设)为:
E [ w i v j ′ ] = 0 E [ w i w j ′ ] = 0 i ≠ j E [ v i v j ′ ] = 0 i ≠ j \begin{aligned} E[w_iv_j'] &=0\\ E[w_iw_j'] &=0\quad i\neq j \\ E[v_iv_j'] &=0\quad i\neq j \end{aligned} E[wivj]E[wiwj]E[vivj]=0=0i=j=0i=j

1.2 扩展卡尔曼滤波器(EKF)

1.) 初始化
给定 k − 1 k-1 k1时刻的状态估计和协方差矩阵
x ^ k − 1 ∣ k − 1 , P k − 1 ∣ k − 1 , Q k − 1 , R k − 1 \hat{x}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1} x^k1k1,Pk1k1,Qk1,Rk1
当为 0 0 0时刻时,滤波器最优初始化为
x 0 ∼ ( x ˉ 0 , P 0 ) , Q 0 , R 0 x_0\sim(\bar{x}_0, P_0),Q_{0},R_{0} x0(xˉ0,P0),Q0,R0
x ^ 0 ∣ 0 = x ˉ 0 P 0 ∣ 0 = P 0 \hat{x}_{0|0}=\bar{x}_0\\P_{0|0}=P_0 x^00=xˉ0P00=P0

2. ) 状态预测
2.1 计算非线性系统方程的雅可比矩阵
F k − 1 = ∂ f ( x k − 1 ) ∂ x k − 1 ∣ x k − 1 = x ^ k − 1 ∣ k − 1 F_{k-1}=\frac{\partial f\left(x_{k-1}\right)}{\partial x_{k-1}}\Big|_{ {x_{k-1}=\hat{x}_{k-1|k-1}}} Fk1=xk1f(xk1)xk1=x^k1k1
2.2 状态一步预测及预测误差协方差阵为
x ^ k ∣ k − 1 = E [ x k ∣ Z k − 1 ] ≈ E [ f k − 1 ( x ^ k − 1 ∣ k − 1 , 0 ) + F k − 1 x ~ k − 1 ∣ k − 1 + w k − 1 ∣ Z k − 1 ] = f k − 1 ( x ^ k − 1 ∣ k − 1 ) P k ∣ k − 1 = cov ( x ~ k ∣ k − 1 ) = F k − 1 P k − 1 ∣ k − 1 F k − 1 ′ + Q k − 1 (2) \textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k-1}&=E\left[ x_k\mid Z^{k-1}\right ]\\&\approx E\left[f_{k-1}\left(\hat{x}_{k-1|k-1},0\right)+F_{k-1}\widetilde{x}_{k-1|k-1}+w_{k-1} \mid Z^{k-1}\right]\\&=f_{k-1}\left(\hat{x}_{k-1|k-1}\right)\\ P_{k\mid k-1}&=\text{cov}\left(\widetilde{x}_{k\mid k-1}\right)= F_{k-1}P_{k-1|k-1}F_{k-1}'+Q_{k-1} \end{aligned}} \tag{2} x^kk1Pkk1=E[xkZk1]E[fk1(x^k1k1,0)+Fk1x k1k1+wk1Zk1]=fk1(x^k1k1)=cov(x kk1)=Fk1Pk1k1Fk1+Qk1(2)
其中系统状态方程在 x ^ k − 1 ∣ k − 1 \hat{x}_{k-1|k-1} x^k1k1的泰勒级数展开为(取一阶)
x k = f k − 1 ( x k − 1 ) + w k − 1 ≈ f k − 1 ( x ^ k − 1 ∣ k − 1 ) + F k − 1 x ~ k − 1 ∣ k − 1 + w k − 1 \begin{aligned} x_k&=f_{k-1}\left(x_{k-1} \right) + w_{k-1}\\ &\approx f_{k-1}\left(\hat{x}_{k-1|k-1}\right)+F_{k-1}\widetilde{x}_{k-1|k-1}+w_{k-1} \end{aligned} xk=fk1(xk1)+wk1fk1(x^k1k1)+Fk1x k1k1+wk1
预测估计误差为
x ~ k ∣ k − 1 = x k − x ^ k ∣ k − 1 ≈ F k − 1 x ~ k − 1 ∣ k − 1 + w k − 1 \tilde{x}_{k\mid k-1}=x_k-\hat{x}_{k|k-1}\approx F_{k-1}\widetilde{x}_{k-1|k-1}+ w_{k-1} x~kk1=xkx^kk1Fk1x k1k1+wk1

3.) 量测预测
3.1 计算量测系统方程的雅可比矩阵
H k = ∂ h ( x k ) ∂ x k ∣ x k = x ^ k ∣ k − 1   H_{k}=\frac{\partial h\left(x_{k}\right)}{\partial x_{k}}\Big|_{ {x_{k}=\hat{x}_{k|k-1}}}\ Hk=xkh(xk)xk=x^kk1 
3.2 量测一步预测、新息协方差、互协方差为
z ^ k ∣ k − 1 = E [ z k ∣ Z k − 1 ] = h ( x ^ k ∣ k − 1 ) S k = cov ( z ~ k ∣ k − 1 ) = H k P k ∣ k − 1 H k ′ + R k C k = cov ( x ~ k ∣ k − 1 , z ~ k ∣ k − 1 ) = P k ∣ k − 1 H k ′ (3) \textcolor{#FF0000}{ \begin{aligned} \hat{z}_{k|k-1}&=E\left[z_k\mid Z^{k-1}\right]= h\left(\hat{x}_{k|k-1}\right)\\ S_k&=\text{cov}\left(\widetilde{z}_{k\mid k-1}\right)= H_{k}P_{k|k-1}H_{k}'+ R_{k}\\ C_k&=\text{cov}\left(\widetilde{x}_{k\mid k-1},\widetilde{z}_{k\mid k-1}\right)= P_{k|k-1}H_{k}' \end{aligned}} \tag{3} z^kk1SkCk=E[zkZk1]=h(x^kk1)=cov(z kk1)=HkPkk1Hk+Rk=cov(x kk1,z kk1)=Pkk1Hk(3)
其中量测方程在 x ^ k ∣ k − 1 \hat{x}_{k|k-1} x^kk1的泰勒级数展开为(取一阶)
z k = h ( x k ) + v k ≈ h k ( x ^ k ∣ k − 1 ) + H k x ~ k ∣ k − 1 + v k − 1 \begin{aligned} z_k&=h\left(x_{k} \right) + v_{k}\\ &\approx h_{k}\left(\hat{x}_{k|k-1}\right)+H_{k}\widetilde{x}_{k|k-1}+v_{k-1} \end{aligned} zk=h(xk)+vkhk(x^kk1)+Hkx kk1+vk1
量测预测误差为
z ~ k ∣ k − 1 = z k − z ^ k ∣ k − 1 ≈ H k z ~ k ∣ k − 1 + v k \tilde{z}_{k\mid k-1}=z_k-\hat{z}_{k|k-1}\approx H_{k}\widetilde{z}_{k|k-1}+ v_{k} z~kk1=zkz^kk1Hkz kk1+vk

4.) 状态更新
4.1 卡尔曼增益为
K k = C k S k − 1 = P k ∣ k − 1 H k ′ ( H k P k ∣ k − 1 H k ′ + R k ) − 1 (4) \textcolor{#FF0000}{ \begin{aligned} K_k&=C_kS_k^{-1}\\ &=P_{k|k-1}H_{k}'( H_{k}P_{k|k-1}H_{k}'+ R_{k})^{-1} \end{aligned}} \tag{4} Kk=CkSk1=Pkk1Hk(HkPkk1Hk+Rk)1(4)
4.2 状态更新为
x ^ k ∣ k = E [ x k ∣ Z k ] = x ^ k ∣ k − 1 + K z ( z k − z ^ k ∣ k − 1 ) P k ∣ k = cov ( x ~ k ∣ k ) = P k ∣ k − 1 − K k S k K k ′ (5) \textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k}&=E\left[ x_k\mid Z^{k}\right ]=\hat{x}_{k|k-1}+K_z\left(z_k-\hat{z}_{k|k-1}\right)\\ P_{k\mid k}&=\text{cov}\left(\widetilde{x}_{k\mid k}\right)=P_{k\mid k-1}-K_kS_kK_k' \end{aligned}} \tag{5} x^kkPkk=E[xkZk]=x^kk1+Kz(zkz^kk1)=cov(x kk)=Pkk1KkSkKk(5)
其中估计误差为
x ~ k ∣ k = x k − x ^ k ∣ k \widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k} x kk=xkx^kk

以上公式(2-5)及为带加性噪声非线性系统的扩展卡尔曼滤波算法。

二、带非加性噪声的扩展卡尔曼滤波算法

2.1 问题描述(离散时间非线性系统描述)

考虑带非加性噪声的一般非线性系统模型,
x k = f ( x k − 1 , w k − 1 ) z k = h ( x k , v k ) (2-1) x_k=f(x_{k-1}, w_{k-1}) \\ z_k=h(x_k, v_k) \tag{2-1} xk=f(xk1wk1)zk=h(xkvk)(2-1)
其中 x k x_k xk k k k时刻的目标状态向量。 z k z_k zk k k k时刻量测向量(传感器数据)。这里不考虑控制器 u k u_k uk w k {w_k} wk v k {v_k} vk分别是过程噪声序列和量测噪声序列,并假设 w k w_k wk v k v_k vk为零均值高斯白噪声,其方差分别为 Q k Q_k Qk R k R_k Rk的高斯白噪声,即 w k ∼ ( 0 , Q k ) w_k\sim(0,Q_k) wk(0,Qk), v k ∼ ( 0 , R k ) v_k\sim(0,R_k) vk(0,Rk),且满足如下关系(线性高斯假设)为:
E [ w i v j ′ ] = 0 E [ w i w j ′ ] = 0 i ≠ j E [ v i v j ′ ] = 0 i ≠ j \begin{aligned} E[w_iv_j'] &=0\\ E[w_iw_j'] &=0\quad i\neq j \\ E[v_iv_j'] &=0\quad i\neq j \end{aligned} E[wivj]E[wiwj]E[vivj]=0=0i=j=0i=j

2.2 带非加性噪声的扩展卡尔曼滤波器(EKF)

1. 初始化
给定 k − 1 k-1 k1时刻的状态估计和协方差矩阵
x ^ k − 1 ∣ k − 1 , P k − 1 ∣ k − 1 , Q k − 1 , R k − 1 \hat{x}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1} x^k1k1,Pk1k1,Qk1,Rk1
当为 0 0 0时刻时,滤波器最优初始化为
x 0 ∼ ( x ˉ 0 , P 0 ) , Q 0 , R 0 x ^ 0 ∣ 0 = x ˉ 0 P 0 ∣ 0 = P 0 x_0\sim(\bar{x}_0, P_0), Q_0,R_0\\\hat{x}_{0|0}=\bar{x}_0\\P_{0|0}=P_0 x0(xˉ0,P0),Q0,R0x^00=xˉ0P00=P0

2. 状态预测
2.1 计算非线性系统方程的雅可比矩阵
F k − 1 x = ∂ f ( x k − 1 , w k − 1 ) ∂ x k − 1 ∣ x k − 1 = x ^ k − 1 ∣ k − 1 w k − 1 = 0 F k − 1 w = ∂ f ( x k − 1 , w k − 1 ) ∂ w k − 1 ∣ x k − 1 = x ^ k − 1 ∣ k − 1 w k − 1 = 0 F_{k-1}^x=\frac{\partial f\left(x_{k-1},w_{k-1}\right)}{\partial x_{k-1}}\Big|_{ {x_{k-1}=\hat{x}_{k-1|k-1}w_{k-1}=0}}\\ F_{k-1}^w=\frac{\partial f\left(x_{k-1},w_{k-1}\right)}{\partial w_{k-1}}\Big|_{ {x_{k-1}=\hat{x}_{k-1|k-1}\\w_{k-1}=0}} Fk1x=xk1f(xk1,wk1)xk1=x^k1k1wk1=0Fk1w=wk1f(xk1,wk1)xk1=x^k1k1wk1=0
2.2 状态一步预测及预测误差协方差阵为
x ^ k ∣ k − 1 = f k − 1 ( x ^ k − 1 ∣ k − 1 , 0 ) P k ∣ k − 1 = F k − 1 x P k − 1 ∣ k − 1 ( F k − 1 x ) ′ + F k − 1 w Q k − 1 ( F k − 1 w ) ′ (2.2) \textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k-1}&=f_{k-1}\left(\hat{x}_{k-1|k-1} ,0\right)\\ P_{k\mid k-1}&= F_{k-1}^xP_{k-1|k-1}(F_{k-1}^x)'+ F_{k-1}^wQ_{k-1}(F_{k-1}^w)' \end{aligned}} \tag{2.2} x^kk1Pkk1=fk1(x^k1k1,0)=Fk1xPk1k1(Fk1x)+Fk1wQk1(Fk1w)(2.2)
其中系统状态方程在 x ^ k − 1 ∣ k − 1 \hat{x}_{k-1|k-1} x^k1k1的泰勒级数展开为(取一阶)
x k = f ( x k − 1 , w k − 1 ) ≈ f ( x ^ k − 1 ∣ k − 1 , 0 ) + F k − 1 x x ~ k − 1 ∣ k − 1 + F k − 1 w w k − 1 \begin{aligned} x_k&=f\left(x_{k-1},w_{k-1} \right) \\ &\approx f\left(\hat{x}_{k-1|k-1}, 0\right)+F_{k-1}^x\widetilde{x}_{k-1|k-1}+F_{k-1}^ww_{k-1} \end{aligned} xk=f(xk1,wk1)f(x^k1k1,0)+Fk1xx k1k1+Fk1wwk1

3. 量测预测
3.1 计算量测系统方程的雅可比矩阵
H k x = ∂ h ( x k , v k ) ∂ x k ∣ x k = x ^ k ∣ k − 1 v k = 0 H k v = ∂ h k ( x k , v k ) ∂ v k ∣ x k = x ^ k ∣ k − 1 v k = 0 H_{k}^x=\frac{\partial h\left(x_{k},v_{k}\right)}{\partial x_{k}}\Big|_{ {x_{k}=\hat{x}_{k|k-1}\\v_{k}=0}} \\ H_{k}^v=\frac{\partial h_{k}\left(x_{k},v_{k}\right)}{\partial v_{k}}\Big|_{ {x_{k}=\hat{x}_{k|k-1}\\v_{k}=0}} Hkx=xkh(xk,vk)xk=x^kk1vk=0Hkv=vkhk(xk,vk)xk=x^kk1vk=0
3.2 量测一步预测、新息协方差、互协方差为
z ^ k ∣ k − 1 = E [ z k ∣ Z k − 1 ] = h ( x ^ k ∣ k − 1 , 0 ) S k = cov ( z ~ k ∣ k − 1 ) = H k x P k ∣ k − 1 ( H k x ) ′ + H k v ( R k H k v ) ′ C k = cov ( x ~ k ∣ k − 1 , z ~ k ∣ k − 1 ) = P k ∣ k − 1 ( H k x ) ′ (2.3) \textcolor{#FF0000}{ \begin{aligned} \hat{z}_{k|k-1}&=E\left[z_k\mid Z^{k-1}\right]= h\left(\hat{x}_{k|k-1},0\right)\\ S_k&=\text{cov}\left(\widetilde{z}_{k\mid k-1}\right)= H_{k}^xP_{k|k-1}(H_{k}^x)'+ H_{k}^v(R_{k}H_{k}^v)'\\ C_k&=\text{cov}\left(\widetilde{x}_{k\mid k-1},\widetilde{z}_{k\mid k-1}\right)= P_{k|k-1}(H_{k}^x)' \end{aligned}} \tag{2.3} z^kk1SkCk=E[zkZk1]=h(x^kk1,0)=cov(z kk1)=HkxPkk1(Hkx)+Hkv(RkHkv)=cov(x kk1,z kk1)=Pkk1(Hkx)(2.3)
其中量测方程在 x ^ k ∣ k − 1 \hat{x}_{k|k-1} x^kk1的泰勒级数展开为(取一阶)
z k = h ( x k , v k ) ≈ h k ( x ^ k ∣ k − 1 , 0 ) + H k x x ~ k ∣ k − 1 + H k v v k − 1 \begin{aligned} z_k&=h\left(x_{k}, v_{k} \right)\\ &\approx h_{k}\left(\hat{x}_{k|k-1},0\right)+H_{k}^x\widetilde{x}_{k|k-1}+H_{k}^vv_{k-1} \end{aligned} zk=h(xk,vk)hk(x^kk1,0)+Hkxx kk1+Hkvvk1

4. 状态更新
4.1 卡尔曼增益为
K k = C k S k − 1 (2.4) \textcolor{#FF0000}{ \begin{aligned} K_k&=C_kS_k^{-1} \end{aligned}} \tag{2.4} Kk=CkSk1(2.4)
4.2 状态更新为
x ^ k ∣ k = E [ x k ∣ Z k ] = x ^ k ∣ k − 1 + K z ( z k − z ^ k ∣ k − 1 ) P k ∣ k = cov ( x ~ k ∣ k ) = P k ∣ k − 1 − K k S k K k ′ (2.5) \textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k}&=E\left[ x_k\mid Z^{k}\right ]=\hat{x}_{k|k-1}+K_z\left(z_k-\hat{z}_{k|k-1}\right)\\ P_{k\mid k}&=\text{cov}\left(\widetilde{x}_{k\mid k}\right)=P_{k\mid k-1}-K_kS_kK_k' \end{aligned}} \tag{2.5} x^kkPkk=E[xkZk]=x^kk1+Kz(zkz^kk1)=cov(x kk)=Pkk1KkSkKk(2.5)
其中估计误差为
x ~ k ∣ k = x k − x ^ k ∣ k \widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k} x kk=xkx^kk

以上公式(2.2-2.5)及为带非加性噪声非线性系统的扩展卡尔曼滤波算法。

三、仿真实验

仿真部分见博客:
扩展卡尔曼滤波EKF在目标跟踪中的应用—仿真部分

https://blog.csdn.net/weixin_44044161/article/details/115329181             

3.1 仿真场景(三维)

EKF仿真代码:三维目标跟踪

https://download.csdn.net/download/weixin_44044161/16239480       

3.2 仿真场景(二维)

EKF仿真代码:二维目标跟踪

https://download.csdn.net/download/weixin_44044161/16239356 

3.3 二维EKF跟踪仿真结果

四、二维EKF跟踪参考代码

说明:
1.二维仿真代码也可以在上面的连接中直接下载,
2.将EKF函数保存,文件名“fun_2EKF.m”
3.将量测函数保存,文件名“measurements.m”
4. 运行下面的主函数
5. 注意将这三个文件保存在一个文件夹下

4.1 主函数

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% created by: Donglin Zhang
% date: 2020/4
% 扩展卡尔曼滤波,目标跟踪  
% 二维目标跟踪问题
% 极坐标两侧
% 线性CV目标模型
% 单雷达
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all; close all; clc;
%% initial parameter
n=4; %状态维数 ;
T=1; %采样时间
M=1; %雷达数目
N=200; %运行总时刻
MC=500; %蒙特卡洛次数
chan=1; %滤波器通道,这里只有一个滤波器
w_mu=[0,0]'; % mean of process noise 
v_mu=[0,0]'; % mean of measurement noise
%% target model
%covariance of process noise
q_x=0.01; %m/s^2
q_y=q_x;
Qk=diag([q_x^2,q_y^2]); 
% state matrix
Fk= [1 T 0 0
     0 T 0 0
     0 0 1 T
     0 0 0 T]; %
 Gk= [ T^2/2  0
       T      0
       0      T^2/2
       0      T ]; %
%量测模型
sigma_r(1)=120;  sigma_b(1)=80e-3; % covariance of measurement noise (radar)
Rk=diag([sigma_r(1)^2, sigma_b(1)^2]);
xp=[0,0,0,0];%雷达位置
%% 定义存储空间
sV=zeros(n,N,MC); % 状态
eV=zeros(n,N,MC,chan); %估计
PV=zeros(n,n,N,MC,chan);%协方差
rV=zeros(2,N,MC,M); % %量测
for i=1:MC
    sprintf('rate of process:%3.1f%%',(2*i)/(4*MC)*100)
    % 初始状态的均值和方差
    x=[1000,20,1000,20]';
    P_0=diag([1e5,10^2,1e5,10^2]); 
    xk_EKF=x;    Pk_EKF=P_0;   % P0|0 x0|0 
    x0=mvnrnd(x,P_0); % 初始状态
    %x0=(x+normrnd(0,0.001)')';
    x=x0';
    for k=1:N
       %% %%%%%%% 目标模型和雷达量测模型%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % 目标模型CV 
        w=mvnrnd(w_mu',Qk)';
        x=Fk*x+Gk*w;
        sV(:,k,i)=x;
        
        % 雷达量测模型,M=1,表示一个雷达
        for m=1:M
            v=normrnd(v_mu,[sigma_r(m); sigma_b(m)]);
            [r,b] = measurements(x,xp);%r=距离,b=角度
            rm=r+v(1);
            bm=b+v(2);
            rV(:,k,i,m)=[rm,bm]';   
        end
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
       %% %%%%%%%%% EKF %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        [xk_EKF,Pk_EKF] = fun_2EKF(xk_EKF,Pk_EKF,Fk,Gk,rV(:,k,i,1),Qk,Rk, xp); 
        PV(:,:,k,i,1)=Pk_EKF;
        eV(:,k,i,1)=xk_EKF;
        %%%%%%%%%%%%%% end filte %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
    end    
    
end
    
figure
plot(sV(1,:,1),sV(3,:,1),'--k',eV(1,:,1,1),eV(3,:,1,1),'-r')
xlabel('m');ylabel('m');
legend('State','EKF')
title('跟踪轨迹')
figure;
ii=1:N;
plot(ii,sV(1,:,1),'--k',ii,eV(1,:,1,1),'r');
xlabel('m');ylabel('m');%X轴的跟踪轨迹
legend('X-轴','EKF')
title('X轴的跟踪轨迹')

%P_real=cell(MC,chan);
P_r=0;
for i=1:MC
    sprintf('rate of process:%3.1f%%',(3*MC+i)/(4*MC)*100)
    for k=1:N
        for c=1:chan
            error(:,c)=sV(:,k,i,1)-eV(:,k,i,c); 
            % RMSE
            error2(:,c)=error(:,c).^2;               
            error2_dis(c)=error2(1,c)+error2(3,c);
            error2_vel(c)=error2(2,c)+error2(4,c);
            position(k,i,c)=error2_dis(c);     
            velocity(k,i,c)=error2_vel(c); 
            % ANEES
            anees(k,i,c)=error(:,c)'/PV(:,:,k,i,c)*error(:,c); 
           
            
        end
    end
end
%% RMSE
for c=1:chan
    rms_position(:,c)=sqrt(sum(position(:,:,c),2)./MC);  
    rms_velocity(:,c)=sqrt(sum(velocity(:,:,c),2)./MC);  
end
figure;%position
plot(ii,rms_position(ii,1),'.-r','LineWidth',0.7);
legend('EKF')
xlabel('t/s');ylabel('RMSE');
title('position-RMS analyze')
figure;%position
plot(ii,rms_velocity(ii,1),'.-r','LineWidth',0.7);
legend('EKF')
xlabel('t/s');ylabel('RMSE');
title('velocity-RMS analyze')    
%% ANEES
for c=1:chan
    sum_anees(:,c)=sum(anees(:,:,c),2);
    ANEES(:,c)=sum_anees(:,c)./(n*MC);
end
figure;
plot(ii,ANEES(ii,1),'.-r','LineWidth',0.7);
legend('EKF')
xlabel('t/s');ylabel('ANEES');
title('ANEES')

4.2 EKF 滤波器函数

function [xk,Pk] = fun_2EKF(xk,Pk,Fk,Gk,rV,Qk,Rk, xp)
%EKF 
zk=rV;% 雷达量测:rm bm 
% 预测
xkk=Fk*xk;
Pkk=Fk*Pk*Fk'+Gk*Qk*Gk';
%量测预测
[rkk,bkk] = measurements(xkk,xp);
zkk=[rkk,bkk]';
%雅可比矩阵
H(1,:)=[(xkk(1)-xp(1))/sqrt((xkk(1)-xp(1))^2+(xkk(3)-xp(3))^2),0, (xkk(3)-xp(3))/sqrt((xkk(1)-xp(1))^2+(xkk(3)-xp(3))^2), 0];
H(2,:)=[-(xkk(3)-xp(3))/((xkk(1)-xp(1))^2+(xkk(3)-xp(3))^2),0,(xkk(1)-xp(1))/((xkk(1)-xp(1))^2+(xkk(3)-xp(3))^2),0];

%update
Sk=H*Pkk*H'+Rk;
Kk=Pkk*H'*inv(Sk);
Ck=Pkk*H';
xk=xkk+Ck*inv(Sk)* (zk-zkk) ;
Pk=Pkk-Ck*inv(Sk)*Ck';

end

4.3 量测方程函数

function [fz1,fz2] = measurements(fx,fxp)
%极坐标量测,二维雷达量测
%距离量测
fz1=sqrt((fx(1)-fxp(1))^2+((fx(3)-fxp(3))^2));
%方位角
fz2=atan2((fx(3)-fxp(3)),(fx(1)-fxp(1)));
end

你可能感兴趣的:(目标跟踪,扩展卡尔曼滤波,信号处理,扩展卡尔曼滤波,目标跟踪,matlab,信号处理,算法)