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

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

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

算法部分见博客：
** 扩展卡尔曼滤波EKF在目标跟踪中的应用—算法部分**

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

作者:823618313@qq.com; WX: ZB823618313
备注：
扩展卡尔曼滤波算法；
目标跟踪matlab仿真实现;
Case: 二维目标跟踪情况和三维目标跟踪情况
代码下载地址如下（分别为二维情形和三维情形）

扩展卡尔曼滤波 机动目标跟踪 EKF

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

扩展卡尔曼滤波EKF匀速圆周运动CT

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

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

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

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

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

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

1.1 问题描述（离散时间非线性系统描述）

考虑带加性噪声的一般非线性系统模型，
$$\begin{gathered} x_k=f(x_{k-1})+w_{k-1} \\ {z}_k=h(x_k)+v_k\text{\hspace{1em}(1)} \end{gathered}$$
其中$x_k$为$k$时刻的目标状态向量。$z_k$为$k$时刻量测向量（传感器数据）。这里不考虑控制器$u_k$。$w_k$和$v_k$分别是过程噪声序列和量测噪声序列，并假设$w_k$和$v_k$为零均值高斯白噪声，其方差分别为$Q_k$和$R_k$的高斯白噪声，即$w_k\sim (0,Q_k)$, $v_k\sim (0,R_k)$，且满足如下关系(线性高斯假设)为：
$$\begin{array}{rl}E[w_i{v}_j^{\prime }]& =0\\ E[w_i{w}_j^{\prime }]& =0i\ne j\\ E[v_i{v}_j^{\prime }]& =0i\ne j\end{array}$$

1.2 扩展卡尔曼滤波器（EKF）

1. 初始化
给定$k-1$时刻的状态估计和协方差矩阵
$${\hat{x}}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1}$$
当为$0$时刻时，滤波器最优初始化为
$$x_0\sim ({\bar{x}}_0,P_0),Q_0,R_0$$
即$$\begin{gathered} {\hat{x}}_{0|0}={\bar{x}}_0 \\ {P}_{0|0}=P_0 \end{gathered}$$
2. 状态预测
2.1 计算非线性系统方程的雅可比矩阵
$$F_{k-1}=\frac{\partial f\left(x_{k-1}\right)}{\partial x_{k-1}}{|}_{x_{k-1}={\hat{x}}_{k-1|k-1}}$$
2.2 状态一步预测及预测误差协方差阵为
$$\begin{array}{rl}{\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}{\tilde{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({\tilde{x}}_{k\mid k-1}\right)=F_{k-1}{P}_{k-1|k-1}{F}_{k-1}^{\prime }+Q_{k-1}\end{array}\text{\hspace{1em}(2)}$$
其中系统状态方程在${\hat{x}}_{k-1|k-1}$的泰勒级数展开为（取一阶）
$$\begin{array}{rl}{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}{\tilde{x}}_{k-1|k-1}+w_{k-1}\end{array}$$
预测估计误差为
$${\tilde{x}}_{k\mid k-1}=x_k-{\hat{x}}_{k|k-1}\approx F_{k-1}{\tilde{x}}_{k-1|k-1}+w_{k-1}$$

3. 量测预测
3.1 计算量测系统方程的雅可比矩阵
$$H_k=\frac{\partial h\left(x_k\right)}{\partial x_k}{|}_{x_k={\hat{x}}_{k|k-1}}$$
3.2 量测一步预测、新息协方差、互协方差为
$$\begin{array}{rl}{\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({\tilde{z}}_{k\mid k-1}\right)=H_k{P}_{k|k-1}{H}_k^{\prime }+R_k\\ C_k& =\text{cov}\left({\tilde{x}}_{k\mid k-1},{\tilde{z}}_{k\mid k-1}\right)=P_{k|k-1}{H}_k^{\prime }\end{array}\text{\hspace{1em}(3)}$$
其中量测方程在${\hat{x}}_{k|k-1}$的泰勒级数展开为（取一阶）
$$\begin{array}{rl}{z}_k& =h\left(x_k\right)+v_k\\ & \approx h_k\left({\hat{x}}_{k|k-1}\right)+H_k{\tilde{x}}_{k|k-1}+v_{k-1}\end{array}$$
量测预测误差为
$${\tilde{z}}_{k\mid k-1}=z_k-{\hat{z}}_{k|k-1}\approx H_k{\tilde{z}}_{k|k-1}+v_k$$

4. 状态更新
4.1 卡尔曼增益为
$$\begin{array}{rl}{K}_k& =C_k{S}_k^{-1}\\ & =P_{k|k-1}{H}_k^{\prime }(H_k{P}_{k|k-1}{H}_k^{\prime }+R_k{)}^{-1}\end{array}\text{\hspace{1em}(4)}$$
4.2 状态更新为
$$\begin{array}{rl}{\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({\tilde{x}}_{k\mid k}\right)=P_{k\mid k-1}-K_k{S}_k{K}_k^{\prime }\end{array}\text{\hspace{1em}(5)}$$
其中估计误差为
$${\tilde{x}}_{k\mid k}=x_k-{\hat{x}}_{k|k}$$

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

三、仿真实验一：EKF二维雷达目标跟踪

3.1 仿真场景（二维）

扩展卡尔曼滤波 机动目标跟踪 EKF

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

扩展卡尔曼滤波EKF匀速圆周运动CT

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

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

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

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

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

目标模型
考虑一各二维的匀速运动目标（CV 模型）：
$$x_{k+1}=F_k{x}_k+G_k{w}_k$$
其中状态向量$x_k=[x_k,{\dot{x}}_k,y_k,{\dot{y}}_k{]}^{\prime }$；噪声为$w_k=[w_x,w_y{]}^{\prime }$；状态转移矩阵$F_k$和噪声驱动矩阵$G_k$如下
$$F_k=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]G_k=\left[\begin{array}{cc}1/2T^2& 0\\ T& 0\\ 0& 1/2T^2\\ 0& T\end{array}\right]$$
采样时间 $T=1s$. 初始状态为 $$\begin{gathered} x_0\sim ({\bar{x}}_0,P_0) \\ {\bar{x}}_0=[10\text{km},200\text{m/s},10\text{km},100\text{m/s}{]}^{\prime } \\ {P}_0=\text{diag}(10^5{\text{m}}^2,10^2{\text{m}}^2/{\text{s}}^2,10^5{\text{m}}^2,10^2{\text{m}}^2/{\text{s}}^2) \end{gathered}$$
过程噪声均值和方差分别为
$$\begin{gathered} {\bar{w}}_k=[0，0{]}^{\prime } \\ {Q}_k=\left[\begin{array}{cc}0.01& 0\\ 0& 0.01\end{array}\right] \end{gathered}$$

如果为非线性目标，则将状态转移矩阵$F_k$代替为雅可比矩阵即可。为了不是一般性这里采用线性模型进行仿真。主要处理目标跟踪，雷达量测存在的非线性滤波问题。

雷达量测模型
在二维情况下，雷达量测为距离和角度
$$\begin{gathered} r_k^m=r_k+{\tilde{r}}_k \\ {b}_k^m=b_k+{\tilde{b}}_k \end{gathered}$$
其中
$$\begin{gathered} r_k=\sqrt{(x_k-x_0{)}^{+}(y_k-y_0{)}^2)} \\ {b}_k={\tan }^{-1}\frac{y_k-y_0}{x_k-x_0} \end{gathered}$$
$[x_0,y_0]$为雷达坐标，一般情况为0。雷达量测为$z_k=[r_k,b_k{]}^{\prime }$。雷达量测方差为
$$R_k=\text{cov}(v_k)=\left[\begin{array}{cc}{\sigma }_r^2& 0\\ 0& {\sigma }_b^2\end{array}\right]$$且${\sigma }_r=120m$， ${\sigma }_b=80mrad$。
性能指标
RMSE(Root mean-squared error):蒙塔卡罗次数$M=500$，${\hat{x}}_{k|k}^i$为第$i$次仿真得到的估计。
$$\text{RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i)({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i{)}^{\prime }}$$
$$\text{Position RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M(x_k-{\hat{x}}_{k|k}^i{)}^2+(y_k-{\hat{y}}_{k|k}^i{)}^2}$$
$$\text{Velocity RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M({\dot{x}}_k-{\hat{\dot{x}}}_{k|k}^i{)}^2+({\dot{y}}_k-{\hat{\dot{y}}}_{k|k}^i{)}^2}$$
ANEES（average normalized estimation error square）,$n$ 为状态维数，${\mathbf{P}}_{k|k}^i$为第$i$次仿真滤波器输出的估计协方差
$$\text{ANEES}(\hat{x})=\frac{1}{Mn}\sum _{i=1}^M({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i{)}^{\prime }({\mathbf{P}}_{k|k}^i{)}^{-1}({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i)$$

3.2 二维EKF跟踪仿真结果

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

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

图1. 跟踪轨迹

图2. x轴跟踪轨迹

图3. Position RMSE

图4. Velocity RMSE

图5. ANEES

四、仿真实验二：EKF三维雷达目标跟踪

4.1 仿真场景（三维）

目标模型
考虑一各三维的匀速运动目标（CV 模型）：
$$x_{k+1}=F_k{x}_k+G_k{w}_k$$
其中状态向量$x_k=[x_k,{\dot{x}}_k,y_k,{\dot{y}}_k,z_k,{\dot{z}}_k{]}^{\prime }$；噪声为$w_k=[w_x,w_y,w_z{]}^{\prime }$；状态转移矩阵$F_k$和噪声驱动矩阵$G_k$如下
$$F_k=\left[\begin{array}{cccccc}1& T& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& T& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right]{\Gamma }_k=\left[\begin{array}{ccc}1/2T^2& 0& 0\\ T& 0& 0\\ 0& 1/2T^2& 0\\ 0& T& \\ 0& 0& 1/2T^2\\ 0& 0& T\end{array}\right]$$
采样时间 $T=1s$. 初始状态为 $$\begin{gathered} x_0\sim ({\bar{x}}_0,P_0) \\ {\bar{x}}_0=[1\text{km},20\text{m/s}，1\text{km},20\text{m/s},1\text{km},20\text{m/s}{]}^{\prime } \\ {P}_0=\text{diag}(10^5{\text{m}}^2,10^2{\text{m}}^2/{\text{s}}^2,10^5{\text{m}}^2,10^2{\text{m}}^2/{\text{s}}^2，10^5{\text{m}}^2,10^2{\text{m}}^2/{\text{s}}^2) \end{gathered}$$
过程噪声均值和方差分别为
$$\begin{gathered} {\bar{w}}_k=[0，0，0{]}^{\prime } \\ {Q}_k=\left[\begin{array}{ccc}0.01& 0& 0\\ 0& 0.01& 0\\ 0& 0& 0.01\end{array}\right] \end{gathered}$$

如果为非线性目标，则将状态转移矩阵$F_k$代替为雅可比矩阵即可。为了不是一般性这里采用线性模型进行仿真。主要处理目标跟踪，雷达量测存在的非线性滤波问题。

雷达量测模型
在二维情况下，雷达量测为距离和角度
$$\begin{gathered} r_k^m=r_k+{\tilde{r}}_k \\ {b}_k^m=b_k+{\tilde{b}}_k \\ {e}_k^m=e_k+{\tilde{e}}_k \end{gathered}$$
其中
$$\begin{gathered} r_k=\sqrt{(x_k-x_0{)}^{+}(y_k-y_0{)}^2)} \\ {b}_k={\tan }^{-1}\frac{y_k-y_0}{x_k-x_0} \\ {e}_k={\tan }^{-1}\frac{z_k-z_0}{\sqrt{(x_k-x_0{)}^2+(y_k-y_0{)}^2}} \end{gathered}$$
$[x_0,y_0,z_0]$为雷达坐标，一般情况为0。雷达量测为$z_k=[r_k,b_k,e_k{]}^{\prime }$。雷达量测方差为
$$R_k=\text{cov}(v_k)=\left[\begin{array}{ccc}{\sigma }_r^2& 0& 0\\ 0& {\sigma }_b^2& 0\\ 0& 0& {\sigma }_e^2\end{array}\right]$$且${\sigma }_r=120m$， ${\sigma }_b=80mrad$, ${\sigma }_e=60mrad$。
性能指标
RMSE(Root mean-squared error):蒙塔卡罗次数$M=500$，${\hat{x}}_{k|k}^i$为第$i$次仿真得到的估计。
$$\text{RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i)({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i{)}^{\prime }}$$
$$\text{Position RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M(x_k-{\hat{x}}_{k|k}^i{)}^2+(y_k-{\hat{y}}_{k|k}^i{)}^2+(z_k-{\hat{z}}_{k|k}^i{)}^2}$$
$$\text{Velocity RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum _{i=1}^M({\dot{x}}_k-{\hat{\dot{x}}}_{k|k}^i{)}^2+({\dot{y}}_k-{\hat{\dot{y}}}_{k|k}^i{)}^2+({\dot{z}}_k-{\hat{\dot{z}}}_{k|k}^i{)}^2}$$
ANEES（average normalized estimation error square）,$n$ 为状态维数，${\mathbf{P}}_{k|k}^i$为第$i$次仿真滤波器输出的估计协方差
$$\text{ANEES}(\hat{x})=\frac{1}{Mn}\sum _{i=1}^M({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i{)}^{\prime }({\mathbf{P}}_{k|k}^i{)}^{-1}({\mathbf{x}}_k-{\hat{\mathbf{x}}}_{k|k}^i)$$

4.2 EKF三维跟踪仿真结果

三维仿真代码和仿真结果见下面链接（自行下载：）
EKF仿真代码：三维目标跟踪

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

五、EKF二维跟踪参考代码

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

5.1 主函数

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% created by: 
% 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); % %量测