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

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

机动目标跟踪/非线性滤波/传感器融合/导航等探讨联系WX: ZB823618313

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

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

作者:823618313@qq.com
备注：
无迹卡尔曼滤波算法；无迹滤波；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

无迹滤波思考：

无迹卡尔曼滤波UKF—目标跟踪中的应用

无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)
- 1、带加性噪声的无迹卡尔曼滤波算法UKF
- - 1.1 问题描述（离散时间非线性系统描述）
  - 1.2 无极变换UT
  - 1.3 无迹卡尔曼滤波器（UKF）
- 2、带非加性噪声的无迹卡尔曼滤波算法（UKF）
- - 2.1 问题描述（离散时间非线性系统描述）
  - 2.2 带非加性噪声的无迹卡尔曼滤波器（UKF）
- 3、仿真实验一：无迹卡尔曼滤波—二维雷达目标跟踪
- - 3.1 二维目标仿真
  - 3.2 二维UKF跟踪仿真结果
- 四、仿真实验二：无迹卡尔曼滤波—三维雷达目标跟踪
- - 4.1 三维目标仿真
  - 4.2 UKF三维目标仿真结果
- 4、二维UKF仿真代码
- - 4.1 主函数

踪中的应用)

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

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

考虑带加性噪声的一般非线性系统模型，
xk=f(xk−1)+wk−1zk=h(xk)+vk(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)
其中xkx_kxk为kkk时刻的目标状态向量。zkz_kzk为kkk时刻量测向量（传感器数据）。这里不考虑控制器uku_kuk。wk{w_k}wk和vk{v_k}vk分别是过程噪声序列和量测噪声序列，并假设wkw_kwk和vkv_kvk为零均值高斯白噪声，其方差分别为QkQ_kQk和RkR_kRk的高斯白噪声，即wk∼(0,Qk)w_k\sim(0,Q_k)wk∼(0,Qk), vk∼(0,Rk)v_k\sim(0,R_k)vk∼(0,Rk)，且满足如下关系(线性高斯假设)为：
E[wivj′]=0E[wiwj′]=0i≠jE[vivj′]=0i≠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 无极变换UT

无迹变换的目的：通过确定性采样得到随机变量x∼(xˉ,Px)x\sim(\bar{x},P_x)x∼(xˉ,Px)的2n+12n+12n+1个sigma点X\mathcal{X}X，将这些sigma点通过非线性函数传播后得到随机变量yyy的sigma点，进而通过加权平均求得随机变量yyy的一二阶矩（i.e.,均值和方差）。也可以理解为，根据x的统计特性，利用确定性采样法获得非线性函数y=f(x)y=f(x)y=f(x)传播后的y的统计特性的。UT变换如下：
X0=xˉXi=xˉ+n+λ[Px]i,i=1,2,⋯,nXn+i=xˉ−n+λ[Px]i,\begin{aligned} &\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{aligned}X0=xˉXi=xˉ+n+λ[Px]i,i=1,2,⋯,nXn+i=xˉ−n+λ[Px]i,
w0m=λn+λw0c=λn+λ+(1−α2+β)wim=wic=12n+2λ,i=1,2,⋯,2n\begin{aligned} &w^m_0=\frac{\lambda}{n+\lambda}\\ &w^c_0=\frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta)\\ &w^m_i=w^c_i=\frac{1}{2n+2\lambda},i=1,2,\cdots,2n \end{aligned}w0m=n+λλw0c=n+λλ+(1−α2+β)wim=wic=2n+2λ1,i=1,2,⋯,2n
其中λ=α2（n+κ）−n\lambda=\alpha^2（{n+\kappa}）-nλ=α2（n+κ）−n，决定sigma点距离xˉ\bar{x}xˉ的距离。10−4≤α<010^{-4}\leq\alpha<010−4≤α<0，κ\kappaκ一般取0或3−n3-n3−n。对于高斯分布，β=2\beta=2β=2为最优，如果为单变量则为0。此外，Px′Px=Px\sqrt{P_x}'\sqrt{P_x}=P_xPx′Px=Px，[Px]i[\sqrt{P_x}]_i[Px]i表示矩阵的第iii列元素。

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

1.） 初始化
步骤一：
给定k−1k-1k−1时刻的状态估计和协方差矩阵
x^k−1∣k−1,Pk−1∣k−1,Qk−1,Rk−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
当为k=0k=0k=0时刻时，假设x0∼(xˉ0,P0),Q0,R0x_0\sim(\bar{x}_0, P_0),Q_{0},R_{0}x0∼(xˉ0,P0),Q0,R0，则滤波器最优初始化为
x^0∣0=xˉ0P0∣0=P0\hat{x}_{0|0}=\bar{x}_0\\P_{0|0}=P_0x^0∣0=xˉ0P0∣0=P0
2. ) 状态预测
步骤二：根据x^k−1∣k−1,Pk−1∣k−1\hat{x}_{k-1|k-1},P_{k-1|k-1}x^k−1∣k−1,Pk−1∣k−1，利用UT变换选取2n+12n+12n+1个sigam点和权值
Xk−1∣k−10=x^k−1∣k−1,i=0Xk−1∣k−1i=x^k−1∣k−1+n+λ[Pk−1∣k−1]i,i=1,2,⋯,nXk−1∣k−1n+i=x^k−1∣k−1−n+λ[Pk−1∣k−1]i,i=1,2,⋯,n\begin{aligned} &\mathcal{X}^0_{k-1|k-1}=\hat{x}_{k-1|k-1},i=0\\ &\mathcal{X}^i_{k-1|k-1}=\hat{x}_{k-1|k-1}+\sqrt{n+\lambda}[\sqrt{P_{k-1|k-1}}]_i,i=1,2,\cdots,n\\ &\mathcal{X}^{n+i}_{k-1|k-1}=\hat{x}_{k-1|k-1}-\sqrt{n+\lambda}[\sqrt{P_{k-1|k-1}}]_i,i=1,2,\cdots,n\\ \end{aligned}Xk−1∣k−10=x^k−1∣k−1,i=0Xk−1∣k−1i=x^k−1∣k−1+n+λ[Pk−1∣k−1]i,i=1,2,⋯,nXk−1∣k−1n+i=x^k−1∣k−1−n+λ[Pk−1∣k−1]i,i=1,2,⋯,n
和权值w0m,wim,w0c,wic，i=1,2,⋯,2nw^m_0,w^m_i, w^c_0, w^c_i，i=1,2,\cdots,2nw0m,wim,w0c,wic，i=1,2,⋯,2n。

步骤三： 根据系统方程传播sigma点
Xk∣k−1i=f(Xk−1∣k−1i)，i=0,1,2,⋯,2n\mathcal{X}^i_{k|k-1}=f(\mathcal{X}^i_{k-1|k-1})，i=0,1,2,\cdots,2nXk∣k−1i=f(Xk−1∣k−1i)，i=0,1,2,⋯,2n
步骤四： 状态一步预测和预测方差
xk∣k−1=∑i=02nwmXk∣k−1iPk∣k−1=∑i=02nwc(Xk∣k−1i−xk∣k−1)(Xk∣k−1i−xk∣k−1)′+Qk\begin{aligned} &x_{k|k-1}=\sum_{i=0}^{2n}w^m\mathcal{X}^i_{k|k-1}\\ &P_{k|k-1}=\sum_{i=0}^{2n}w^c(\mathcal{X}^i_{k|k-1}-x_{k|k-1})(\mathcal{X}^i_{k|k-1}-x_{k|k-1})'+Q_k \end{aligned}xk∣k−1=i=0∑2nwmXk∣k−1iPk∣k−1=i=0∑2nwc(Xk∣k−1i−xk∣k−1)(Xk∣k−1i−xk∣k−1)′+Qk
3. ) 状态更新
步骤五： 将Xk∣k−1i\mathcal{X}^i_{k|k-1}Xk∣k−1i点通过量测方程传播，得到量测预测sigma点
Zk∣k−1i=h(Xk∣k−1i)，i=0,1,2,⋯,2n\mathcal{Z}^i_{k|k-1}=h(\mathcal{X}^i_{k|k-1})，i=0,1,2,\cdots,2nZk∣k−1i=h(Xk∣k−1i)，i=0,1,2,⋯,2n
步骤六： 量测预测，量测预测误差方差（新息方差），状态与量测互协方差，增益
zk∣k−1=∑i=02nwmZk∣k−1iSk=∑i=02nwc(Zk∣k−1i−zk∣k−1)(Zk∣k−1i−zk∣k−1)′+RkCk=∑i=02nwc(Xk∣k−1i−xk∣k−1)(Zk∣k−1i−zk∣k−1)′Kk=CkSk−1\begin{aligned} &z_{k|k-1}=\sum_{i=0}^{2n}w^m\mathcal{Z}^i_{k|k-1}\\ &S_{k}=\sum_{i=0}^{2n}w^c(\mathcal{Z}^i_{k|k-1}-z_{k|k-1})(\mathcal{Z}^i_{k|k-1}-z_{k|k-1})'+R_k\\ &C_{k}=\sum_{i=0}^{2n}w^c(\mathcal{X}^i_{k|k-1}-x_{k|k-1})(\mathcal{Z}^i_{k|k-1}-z_{k|k-1})'\\ &K_k=C_{k}S_{k}^{-1} \end{aligned}zk∣k−1=i=0∑2nwmZk∣k−1iSk=i=0∑2nwc(Zk∣k−1i−zk∣k−1)(Zk∣k−1i−zk∣k−1)′+RkCk=i=0∑2nwc(Xk∣k−1i−xk∣k−1)(Zk∣k−1i−zk∣k−1)′Kk=CkSk−1
步骤七：

x^k∣k=E[xk∣Zk]=x^k∣k−1+Kz(zk−z^k∣k−1)Pk∣k=cov(x~k∣k)=Pk∣k−1−KkSkKk′(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(xk∣k)=Pk∣k−1−KkSkKk′(5)
其中估计误差为
x~k∣k=xk−x^k∣k\widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k}xk∣k=xk−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 模型：
xk+1=Fkxk+Gkwkx_{k+1}=F_kx_k+G _kw_kxk+1=Fkxk+Gkwk
其中状态xk=[xk,x˙k,yk,y˙k]′x_k=[x_k,\dot{x}_k,y_k,\dot{y}_k]'xk=[xk,x˙k,yk,y˙k]′，[xk,yk′[x_k,y_k'[xk,yk′为目标位置，[dotxk,y˙k]′[dot{x}_k,\dot{y}_k]'[dotxk,y˙k]′为目标速度；噪声为wk=[wx,wy]′w_k=[w_x,w_y]'wk=[wx,wy]′；状态转移矩阵FkF_kFk和噪声驱动矩阵GkG_kGk如下
Fk=[1T000100001T0001]Gk=[1/2T20T001/2T20T]F_k=\begin{bmatrix}1 & T & 0 & 0 \\0 & 1 & 0 & 0 \\0 & 0 & 1 & T\\0 & 0 & 0 & 1\end{bmatrix} \qquad G_k=\begin{bmatrix}1/2T^2 & 0 \\T & 0 \\0 & 1/2T^2 \\0 & T \end{bmatrix}Fk=⎣⎡1000T100001000T1⎦⎤Gk=⎣⎡1/2T2T00001/2T2T⎦⎤
初始状态为 x0∼(xˉ0,P0)xˉ0=[100m,15m/s,100m,15m/s]′P0=diag(103m2,10m2/s2,103m2,102m/s2)x_0\sim(\bar{x}_0,P_0)\\\bar{x}_{0}=[100m, 15\text{m/s} ,100m, 15\text{m/s}]'\\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)x0∼(xˉ0,P0)xˉ0=[100m,15m/s,100m,15m/s]′P0=diag(103m2,10m2/s2,103m2,102m/s2)
过程噪声噪声均值为0，即wˉk=[0，0]′\bar{w}_k=[0，0]'wˉk=[0，0]′。方差分别为
Qk=[0.12000.12]Q_k=\begin{bmatrix}0.1^2 & 0 \\0 & 0.1^2 \end{bmatrix}Qk=[0.12000.12]
采样时间 T=1sT=1sT=1s.

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

雷达量测模型
在二维情况下，雷达量测为距离，方位角
rkm=rk+r~kbkm=bk+b~k{r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_krkm=rk+r~kbkm=bk+b~k
其中
rk=hr(xk,vk)=(xk−x0)+(yk−y0)2)bk=hb(xk,vk)=tan⁡−1yk−y0xk−x0r_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}}\\ rk=hr(xk,vk)=(xk−x0)+(yk−y0)2)bk=hb(xk,vk)=tan−1xk−x0yk−y0
[x0,y0][x_0,y_0][x0,y0]为传感器（雷达）坐标，一般情况为0。雷达量测为zk=[rk,bk]′z_k=[r_k,b_k]'zk=[rk,bk]′。雷达量测方差为
Rk=cov(vk)=[σr200σb2]R_k=\text{cov}(v_k)=\begin{bmatrix}\sigma_r^2 & 0 \\0 & \sigma_b^2 \end{bmatrix}Rk=cov(vk)=[σr200σb2]且σr=80m\sigma_r=80mσr=80m， σb=60mrad\sigma_b=60mradσb=60mrad。
性能指标
蒙塔卡罗次数M=500M=500M=500，x^k∣ki\hat{x}_{k|k}^ix^k∣ki为第iii次仿真得到的估计，RMSE(Root mean-squared error):
RMSE(x^)=1M∑i=1M(xk−x^k∣ki)(xk−x^k∣ki)′\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)'}RMSE(x^)=M1i=1∑M(xk−x^k∣ki)(xk−x^k∣ki)′
其中位置RMSE和速度RMSE分别取对应状态x~k∣k\tilde{x}_{k|k}x~k∣k的位置项和速度项，加和，具体公式见博客《扩展卡尔曼滤波EKF在目标跟踪中的应用—仿真部分》

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

3.2 二维UKF跟踪仿真结果

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

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

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

4.1 三维目标仿真

目标模型
为了方便，考虑一个二维匀速运动目标，即CV 模型：
xk+1=Fkxk+Gkwkx_{k+1}=F_kx_k+G _kw_kxk+1=Fkxk+Gkwk
其中状态向量xk=[xk,x˙k,yk,y˙k,zk,z˙k]′x_k=[x_k,\dot{x}_k,y_k,\dot{y}_k,z_k,\dot{z}_k]'xk=[xk,x˙k,yk,y˙k,zk,z˙k]′，[xk,yk,zk]′[x_k,y_k,z_k]'[xk,yk,zk]′为目标位置，[dotxk,y˙k,z˙k]′[dot{x}_k,\dot{y}_k,\dot{z}_k]'[dotxk,y˙k,z˙k]′为目标速度；；噪声为wk=[wx,wy,wz]′w_k=[w_x,w_y,w_z]'wk=[wx,wy,wz]′；状态转移矩阵FkF_kFk和噪声驱动矩阵GkG_kGk如下
Fk=[1T0000010000001T0000010000001T000001]Γk=[1/2T200T0001/2T200T001/2T200T]F_k=\begin{bmatrix}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{bmatrix} \qquad\varGamma_k=\begin{bmatrix}1/2T^2 & 0 & 0 \\T & 0 & 0 \\0 & 1/2T^2 & 0 \\0 & T & \\0 & 0 & 1/2T^2 \\0 & 0 & T\end{bmatrix}Fk=⎣⎡100000T1000000100000T1000000100000T1⎦⎤Γk=⎣⎡1/2T2T0000001/2T2T000001/2T2T⎦⎤
初始状态为 x0∼(xˉ0,P0)xˉ0=[12km,31m/s，13km,20m/s,11km,21m/s]′P0=diag(105m2,102m2/s2,105m2,102m2/s2，105m2,102m2/s2)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}]'\\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)x0∼(xˉ0,P0)xˉ0=[12km,31m/s，13km,20m/s,11km,21m/s]′P0=diag(105m2,102m2/s2,105m2,102m2/s2，105m2,102m2/s2)
过程噪声均值为0，即wˉk=[0，0，0]′\bar{w}_k=[0，0， 0]'wˉk=[0，0，0]′。方差为
Qk=[0.010000.010000.01]Q_k=\begin{bmatrix}0.01 & 0& 0 \\0 & 0.01 & 0\\0&0 & 0.01 \end{bmatrix}Qk=⎣⎡0.010000.010000.01⎦⎤
采样时间 T=1sT=1sT=1s.

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

雷达量测模型
在二维情况下，雷达量测为距离，方位角，俯仰角
rkm=rk+r~kbkm=bk+b~kekm=ek+e~k{r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_k\\ e^m_k=e_k+\tilde{e}_krkm=rk+r~kbkm=bk+b~kekm=ek+e~k
其中
rk=hr(xk,vk)=(xk−x0)+(yk−y0)2)bk=hb(xk,vk)=tan⁡−1yk−y0xk−x0ek=he(xk,vk)=tan⁡−1zk−z0(xk−x0)2+(yk−y0)2r_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}}}\\ rk=hr(xk,vk)=(xk−x0)+(yk−y0)2)bk=hb(xk,vk)=tan−1xk−x0yk−y0ek=he(xk,vk)=tan−1(xk−x0)2+(yk−y0)2zk−z0
[x0,y0,z0][x_0,y_0,z_0][x0,y0,z0]为传感器（雷达）坐标，一般情况为0。雷达量测为zk=[rk,bk,ek]′z_k=[r_k,b_k,e_k]'zk=[rk,bk,ek]′。量测方差为
Rk=cov(vk)=[σr2000σb2000σe2]R_k=\text{cov}(v_k)=\begin{bmatrix}\sigma_r^2 & 0 &0\\0 & \sigma_b^2 &0\\0&0& \sigma_e^2 \end{bmatrix}Rk=cov(vk)=⎣⎡σr2000σb2000σe2⎦⎤且σr=130m\sigma_r=130mσr=130m， σb=90mrad\sigma_b=90mradσb=90mrad, σe=70mrad\sigma_e=70mradσe=70mrad。
性能指标
蒙塔卡罗次数M=500M=500M=500，x^k∣ki\hat{x}_{k|k}^ix^k∣ki为第iii次仿真得到的估计，RMSE(Root mean-squared error):
RMSE(x^)=1M∑i=1M(xk−x^k∣ki)(xk−x^k∣ki)′\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)'}RMSE(x^)=M1i=1∑M(xk−x^k∣ki)(xk−x^k∣ki)′
其中位置RMSE和速度RMSE分别取对应状态x~k∣k\tilde{x}_{k|k}x~k∣k的位置项和速度项，加和。具体公式见博客《扩展卡尔曼滤波EKF在目标跟踪中的应用—仿真部分》

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

4.2 UKF三维目标仿真结果

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

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

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

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

无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(算法部分) 原创不易,路过的各位大佬请点个赞机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313 仿真部分见博客: [无迹卡 ...
容积卡尔曼滤波CKF—目标跟踪中的应用(仿真部分—II)
容积卡尔曼滤波CKF-目标跟踪中的应用(算法部分-II) 原创不易,路过的各位大佬请点个赞机动目标跟踪/非线性滤波/传感器融合/导航等探讨联系WX: ZB823618313 作者:823618313 ...
容积卡尔曼滤波CKF—目标跟踪中的应用(算法部分—I)
容积卡尔曼滤波CKF-目标跟踪中的应用(算法部分) 原创不易,路过的各位大佬请点个赞机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313 作者:823618313@ ...
【卡尔曼滤波】卡尔曼滤波在雷达目标跟踪中的应用仿真matlab源码
1 模型 [摘要]目标跟踪问题的应用背景是雷达数据处理,即雷达在搜索到目标并记录目标的位置数据,对测量到的目标位置数据(称为点迹)进行处理,自动形成航迹,并对目标在下一时刻的位置进行预测.本文简要讨论 ...
MATLAB应用实战系列（七十六）-【仿真应用】卡尔曼滤波在雷达目标跟踪中的应用仿真（附matlab代码）
1 模型目标跟踪问题的应用背景是雷达数据处理,即雷达在搜索到目标并记录目标的位置数据,对测量到的目标位置数据(称为点迹)进行处理,自动形成航迹,并对目标在下一时刻的位置进行预测.本文简要讨论了用Ka ...
无迹卡尔曼滤波（UKF）在单观测站目标跟踪中的应用
假定目标做匀速直线运动,在单个观测站对目标进行观测的前提下,再假设目标的初始状态已知.目标的运动方程可以写成如下形式: 其中: 设采样时间间隔T=1s,运行时间为N=60s,W(k)的均方差为,为一个 ...
交互式多模型-扩展卡尔曼滤波IMM-EKF——机动目标跟踪中的应用
交互式多模型-扩展卡尔曼滤波IMM-EKF--机动目标跟踪中的应用原创不易,路过的各位大佬请点个赞针对机动目标跟踪的探讨.技术支持欢迎联系,也可以站内私信 WX: ZB823618313 机动目标 ...
交互式多模型算法IMM——机动目标跟踪中的应用
机动目标跟踪--交互式多模型算法IMM 原创不易,路过的各位大佬请点个赞 WX: ZB823618313 机动目标跟踪--交互式多模型算法IMM 机动目标跟踪--交互式多模型算法IMM 1. 对机动目 ...
交互式多模型-粒子滤波IMM-PF—在机动目标跟踪中的应用/matlab实现
交互式多模型-粒子滤波IMM-PF-在机动目标跟踪中的应用/matlab实现原创不易,路过的各位大佬请点个赞 WX: ZB823618313 交互式多模型-粒子滤波IMM-PF-在机动目标跟踪中的应 ...

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

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

无迹卡尔曼滤波UKF—目标跟踪中的应用

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

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

1.2 无极变换UT

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

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

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

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

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

3.1 二维目标仿真

3.2 二维UKF跟踪仿真结果

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

4.1 三维目标仿真

4.2 UKF三维目标仿真结果

4、二维UKF仿真代码

4.1 主函数

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

最新文章

热门文章