无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)

无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)

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

算法部分见博客：
[无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分]

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

作者:823618313@qq.com; WX: ZB823618313
备注：
无迹卡尔曼滤波算法；无迹滤波；Uncented Filter
两种UKF算法：加性噪声UKF和非加性噪声UKF
matlab实现;
目标跟踪仿真
Case: 二维目标跟踪情况和三维目标跟踪情况
代码下载地址如下（分别为二维情形和三维情形）
UKF仿真代码：二维目标跟踪

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

无迹卡尔曼滤波UKF 匀速转弯CT模型

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

UKF仿真代码：二维目标跟踪——RMSE

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

UKF仿真代码：三维目标跟踪——RMSE

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

无迹滤波思考：

踪中的应用)

1、带加性噪声的无迹卡尔曼滤波算法UKF

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 无极变换UT

无迹变换的目的：通过确定性采样得到随机变量$x\sim (\bar{x},P_x)$的$2n+1$个sigma点$\mathcal{X}$，将这些sigma点通过非线性函数传播后得到随机变量$y$的sigma点，进而通过加权平均求得随机变量$y$的一二阶矩（i.e.,均值和方差）。也可以理解为，根据x的统计特性，利用确定性采样法获得非线性函数$y=f(x)$传播后的y的统计特性的。UT变换如下：
$$\begin{array}{rl}& {\mathcal{X}}^0=\bar{x}\\ & {\mathcal{X}}^i=\bar{x}+\sqrt{n+\lambda }[\sqrt{P_x}{]}_i,i=1,2,\cdots ,n\\ & {\mathcal{X}}^{n+i}=\bar{x}-\sqrt{n+\lambda }[\sqrt{P_x}{]}_i,\end{array}$$
$$\begin{array}{rl}& w_0^m=\frac{\lambda }{n+\lambda }\\ & w_0^c=\frac{\lambda }{n+\lambda }+(1-{\alpha }^2+\beta )\\ & w_i^m=w_i^c=\frac{1}{2n+2\lambda },i=1,2,\cdots ,2n\end{array}$$
其中$\lambda ={\alpha }^2（n+\kappa ）-n$，决定sigma点距离$\bar{x}$的距离。$10^{-4}\le \alpha <0$，$\kappa$一般取0或$3-n$。对于高斯分布，$\beta =2$为最优，如果为单变量则为0。此外，${\sqrt{P_x}}^{\prime }\sqrt{P_x}=P_x$，$[\sqrt{P_x}{]}_i$表示矩阵的第$i$列元素。

1.3 无迹卡尔曼滤波器（UKF）

1.） 初始化
步骤一：
给定$k-1$时刻的状态估计和协方差矩阵
$${\hat{x}}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1}$$
当为$k=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. ) 状态预测
步骤二： 根据${\hat{x}}_{k-1|k-1},P_{k-1|k-1}$，利用UT变换选取$2n+1$个sigam点和权值
$$\begin{array}{rl}& {\mathcal{X}}_{k-1|k-1}^0={\hat{x}}_{k-1|k-1},i=0\\ & {\mathcal{X}}_{k-1|k-1}^i={\hat{x}}_{k-1|k-1}+\sqrt{n+\lambda }[\sqrt{P_{k-1|k-1}}{]}_i,i=1,2,\cdots ,n\\ & {\mathcal{X}}_{k-1|k-1}^{n+i}={\hat{x}}_{k-1|k-1}-\sqrt{n+\lambda }[\sqrt{P_{k-1|k-1}}{]}_i,i=1,2,\cdots ,n\end{array}$$
和权值$w_0^m,w_i^m,w_0^c,w_i^c，i=1,2,\cdots ,2n$。

步骤三： 根据系统方程传播sigma点
$${\mathcal{X}}_{k|k-1}^i=f({\mathcal{X}}_{k-1|k-1}^i)，i=0,1,2,\cdots ,2n$$
步骤四： 状态一步预测和预测方差
$$\begin{array}{rl}& x_{k|k-1}=\sum _{i=0}^{2n}{w}^m{\mathcal{X}}_{k|k-1}^i\\ & P_{k|k-1}=\sum _{i=0}^{2n}{w}^c({\mathcal{X}}_{k|k-1}^i-x_{k|k-1})({\mathcal{X}}_{k|k-1}^i-x_{k|k-1}{)}^{\prime }+Q_k\end{array}$$
3. ) 状态更新
步骤五： 将${\mathcal{X}}_{k|k-1}^i$点通过量测方程传播，得到量测预测sigma点
$${\mathcal{Z}}_{k|k-1}^i=h({\mathcal{X}}_{k|k-1}^i)，i=0,1,2,\cdots ,2n$$
步骤六： 量测预测，量测预测误差方差（新息方差），状态与量测互协方差，增益
$$\begin{array}{rl}& z_{k|k-1}=\sum _{i=0}^{2n}{w}^m{\mathcal{Z}}_{k|k-1}^i\\ & S_k=\sum _{i=0}^{2n}{w}^c({\mathcal{Z}}_{k|k-1}^i-z_{k|k-1})({\mathcal{Z}}_{k|k-1}^i-z_{k|k-1}{)}^{\prime }+R_k\\ & C_k=\sum _{i=0}^{2n}{w}^c({\mathcal{X}}_{k|k-1}^i-x_{k|k-1})({\mathcal{Z}}_{k|k-1}^i-z_{k|k-1}{)}^{\prime }\\ & K_k=C_k{S}_k^{-1}\end{array}$$
步骤七：

$$\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）及为带加性噪声非线性系统的无迹卡尔曼滤波算法。

2、带非加性噪声的无迹卡尔曼滤波算法（UKF）

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

2.2 带非加性噪声的无迹卡尔曼滤波器（UKF）

3、仿真实验一：无迹卡尔曼滤波—二维雷达目标跟踪

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

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

无迹卡尔曼滤波UKF 匀速转弯CT模型

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

UKF仿真代码：二维目标跟踪——RMSE

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

UKF仿真代码：三维目标跟踪——RMSE

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

3.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{]}^{\prime }$，$[x_k,y_k^{\prime }$为目标位置，$[dot{x}_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]$$
初始状态为 $$\begin{gathered} x_0\sim ({\bar{x}}_0,P_0) \\ {\bar{x}}_0=[100m,15\text{m/s},100m,15\text{m/s}{]}^{\prime } \\ {P}_0=\text{diag}(10^3{\text{m}}^2,10{\text{m}}^2/{\text{s}}^2,10^3{\text{m}}^2,10^2\text{m}/{\text{s}}^2) \end{gathered}$$
过程噪声噪声均值为0，即${\bar{w}}_k=[0，0{]}^{\prime }$。方差分别为
$$Q_k=\left[\begin{array}{cc}0.1^2& 0\\ 0& 0.1^2\end{array}\right]$$
采样时间 $T=1s$.

尽管这里的目标模型为线性，在使用UKF预测时，可以用两种方法：xkk=Fkxk,Pkk=FkPkFk’+GkQk*Gk’；或者采用sigma取点来计算该线性模型前两阶矩（线性系统是特殊的非线性系统）

雷达量测模型
在二维情况下，雷达量测为距离，方位角
$$\begin{gathered} r_k^m=r_k+{\tilde{r}}_k \\ {b}_k^m=b_k+{\tilde{b}}_k \end{gathered}$$
其中
$$\begin{gathered} r_k=h_r(x_k,v_k)=\sqrt{(x_k-x_0{)}^{+}(y_k-y_0{)}^2)} \\ {b}_k=h_b(x_k,v_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=80m$， ${\sigma }_b=60mrad$。
性能指标
蒙塔卡罗次数$M=500$，${\hat{x}}_{k|k}^i$为第$i$次仿真得到的估计，RMSE(Root mean-squared error):
$$\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 }}$$
其中位置RMSE和速度RMSE分别取对应状态${\tilde{x}}_{k|k}$的位置项和速度项，加和，具体公式见博客《扩展卡尔曼滤波EKF在目标跟踪中的应用—仿真部分》

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

3.2 二维UKF跟踪仿真结果

UKF仿真代码：二维目标仿真

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

四、仿真实验二：无迹卡尔曼滤波—三维雷达目标跟踪

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 }$，$[x_k,y_k,z_k{]}^{\prime }$为目标位置，$[dot{x}_k,{\dot{y}}_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]$$
初始状态为 $$\begin{gathered} x_0\sim ({\bar{x}}_0,P_0) \\ {\bar{x}}_0=[12\text{km},31\text{m/s}，13\text{km},20\text{m/s},11\text{km},21\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}$$
过程噪声均值为0，即${\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]$$
采样时间 $T=1s$.

尽管这里的目标模型为线性，在使用UKF预测时，可以用两种方法：xkk=Fkxk,Pkk=FkPkFk’+GkQk*Gk’；或者采用sigma取点来计算该线性模型前两阶矩（线性系统是特殊的非线性系统）

雷达量测模型
在二维情况下，雷达量测为距离，方位角，俯仰角
$$\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=h_r(x_k,v_k)=\sqrt{(x_k-x_0{)}^{+}(y_k-y_0{)}^2)} \\ {b}_k=h_b(x_k,v_k)={\tan }^{-1}\frac{y_k-y_0}{x_k-x_0} \\ {e}_k=h_e(x_k,v_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=130m$， ${\sigma }_b=90mrad$, ${\sigma }_e=70mrad$。
性能指标
蒙塔卡罗次数$M=500$，${\hat{x}}_{k|k}^i$为第$i$次仿真得到的估计，RMSE(Root mean-squared error):
$$\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 }}$$
其中位置RMSE和速度RMSE分别取对应状态${\tilde{x}}_{k|k}$的位置项和速度项，加和。具体公式见博客《扩展卡尔曼滤波EKF在目标跟踪中的应用—仿真部分》

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

4.2 UKF三维目标仿真结果

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

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

4、二维UKF仿真代码

说明:
2.将UKF函数保存，文件名“fun_2UKF.m”
3.将量测函数保存，文件名“measurements.m”
4. 运行下面的主函数
5. 注意将这三个文件保存在一个文件夹下

4.1 主函数

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% created by: 
% date: 2020/4
% 无迹卡尔曼滤波，目标跟踪  
% 二维目标跟踪问题
% 线性CV目标模型
% 单雷达
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all; close all; clc;
%% initial parameter
n=4; %状态维数 ;
T=1; %采样时间
M=1; %雷达数目
N=200; %运行总时刻
MC=100; %蒙特卡洛次数
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]; %