扩展卡尔曼滤波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(xk−1)+wk−1zk=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 k−1时刻的状态估计和协方差矩阵
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^k−1∣k−1,Pk−1∣k−1,Qk−1,Rk−1
当为 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^0∣0=xˉ0P0∣0=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}}} Fk−1=∂xk−1∂f(xk−1)∣∣∣xk−1=x^k−1∣k−1
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^k∣k−1Pk∣k−1=E[xk∣Zk−1]≈E[fk−1(x^k−1∣k−1,0)+Fk−1x k−1∣k−1+wk−1∣Zk−1]=fk−1(x^k−1∣k−1)=cov(x k∣k−1)=Fk−1Pk−1∣k−1Fk−1′+Qk−1(2)
其中系统状态方程在 x ^ k − 1 ∣ k − 1 \hat{x}_{k-1|k-1} x^k−1∣k−1的泰勒级数展开为（取一阶）
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=fk−1(xk−1)+wk−1≈fk−1(x^k−1∣k−1)+Fk−1x k−1∣k−1+wk−1
预测估计误差为
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~k∣k−1=xk−x^k∣k−1≈Fk−1x k−1∣k−1+wk−1
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=∂xk∂h(xk)∣∣∣xk=x^k∣k−1
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^k∣k−1SkCk=E[zk∣Zk−1]=h(x^k∣k−1)=cov(z k∣k−1)=HkPk∣k−1Hk′+Rk=cov(x k∣k−1,z k∣k−1)=Pk∣k−1Hk′(3)
其中量测方程在 x ^ k ∣ k − 1 \hat{x}_{k|k-1} x^k∣k−1的泰勒级数展开为（取一阶）
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)+vk≈hk(x^k∣k−1)+Hkx k∣k−1+vk−1
量测预测误差为
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~k∣k−1=zk−z^k∣k−1≈Hkz k∣k−1+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=CkSk−1=Pk∣k−1Hk′(HkPk∣k−1Hk′+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^k∣kPk∣k=E[xk∣Zk]=x^k∣k−1+Kz(zk−z^k∣k−1)=cov(x k∣k)=Pk∣k−1−KkSkKk′(5)
其中估计误差为
x ~ k ∣ k = x k − x ^ k ∣ k \widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k} x k∣k=xk−x^k∣k
以上公式（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(xk−1，wk−1)zk=h(xk，vk)(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 k−1时刻的状态估计和协方差矩阵
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^k−1∣k−1,Pk−1∣k−1,Qk−1,Rk−1
当为 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^0∣0=xˉ0P0∣0=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}} Fk−1x=∂xk−1∂f(xk−1,wk−1)∣∣∣xk−1=x^k−1∣k−1wk−1=0Fk−1w=∂wk−1∂f(xk−1,wk−1)∣∣∣xk−1=x^k−1∣k−1wk−1=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^k∣k−1Pk∣k−1=fk−1(x^k−1∣k−1,0)=Fk−1xPk−1∣k−1(Fk−1x)′+Fk−1wQk−1(Fk−1w)′(2.2)
其中系统状态方程在 x ^ k − 1 ∣ k − 1 \hat{x}_{k-1|k-1} x^k−1∣k−1的泰勒级数展开为（取一阶）
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(xk−1,wk−1)≈f(x^k−1∣k−1,0)+Fk−1xx k−1∣k−1+Fk−1wwk−1
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=∂xk∂h(xk,vk)∣∣∣xk=x^k∣k−1vk=0Hkv=∂vk∂hk(xk,vk)∣∣∣xk=x^k∣k−1vk=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^k∣k−1SkCk=E[zk∣Zk−1]=h(x^k∣k−1,0)=cov(z k∣k−1)=HkxPk∣k−1(Hkx)′+Hkv(RkHkv)′=cov(x k∣k−1,z k∣k−1)=Pk∣k−1(Hkx)′(2.3)
其中量测方程在 x ^ k ∣ k − 1 \hat{x}_{k|k-1} x^k∣k−1的泰勒级数展开为（取一阶）
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^k∣k−1,0)+Hkxx k∣k−1+Hkvvk−1
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=CkSk−1(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^k∣kPk∣k=E[xk∣Zk]=x^k∣k−1+Kz(zk−z^k∣k−1)=cov(x k∣k)=Pk∣k−1−KkSkKk′(2.5)
其中估计误差为
x ~ k ∣ k = x k − x ^ k ∣ k \widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k} x k∣k=xk−x^k∣k
以上公式（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