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

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

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

仿真部分见博客：

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

链接: /weixin_44044161/article/details/115390660.

作者:823618313@

备注：

无迹卡尔曼滤波算法；无迹滤波；Uncented Filter

两种UKF算法：加性噪声UKF和非加性噪声UKF

matlab实现;

目标跟踪仿真

Case: 二维目标跟踪情况和三维目标跟踪情况

代码下载地址如下（分别为二维情形和三维情形）

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

/download/weixin_44044161/85401310

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

/download/weixin_44044161/85402056

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

/download/weixin_44044161/85123963

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

/download/weixin_44044161/85124056

无迹滤波思考：

CKF和UKF 总结：

当取κ=0\kappa=0κ=0时， CKF 和 UKF 的估计精度相同，但鉴于 CKF 采样点少，实时性

比 UKF 好，故应选用 CKF 滤波算法；

当n≤2n\leq2n≤2时即低维非线性系统， UKF 的估计精度高于 CKF，应选用 UKF 滤波

算法；

当n=2n=2n=2时的非线性系统， UKF 及 CKF 的估计精度相同，但 CKF 的实时性更

好，应选用 CKF 滤波算法；

当 n≥3n\geq3n≥3时即高维非线性系统， CKF 的估计精度高于 UKF，应选用 CKF 滤波算法。

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

无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分)1、带加性噪声的无迹卡尔曼滤波算法UKF1.1 问题描述（离散时间非线性系统描述）1.2 无极变换UT1.3 无迹卡尔曼滤波器（UKF）二、带非加性噪声的无迹卡尔曼滤波算法（UKF）2.1 问题描述（离散时间非线性系统描述）2.2 带非加性噪声的无迹卡尔曼滤波器（UKF）三、仿真实验3.1 仿真场景（三维）3.2 仿真场景（二维）3.3 二维UKF跟踪仿真结果四、二维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−1​zk​=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[wi​vj′​]E[wi​wj′​]E[vi​vj′​]​=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ˉ0​P0∣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∑2n​wmXk∣k−1i​Pk∣k−1​=i=0∑2n​wc(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∑2n​wmZk∣k−1i​Sk​=i=0∑2n​wc(Zk∣k−1i​−zk∣k−1​)(Zk∣k−1i​−zk∣k−1​)′+Rk​Ck​=i=0∑2n​wc(Xk∣k−1i​−xk∣k−1​)(Zk∣k−1i​−zk∣k−1​)′Kk​=Ck​Sk−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∣k​Pk∣k​​=E[xk​∣Zk]=x^k∣k−1​+Kz​(zk​−z^k∣k−1​)=cov(xk∣k​)=Pk∣k−1​−Kk​Sk​Kk′​​(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）及为带加性噪声非线性系统的无迹卡尔曼滤波算法。

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

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

考虑带非加性噪声的一般非线性系统模型，

xk=f(xk−1，wk−1)zk=h(xk，vk)(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)

其中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[wi​vj′​]E[wi​wj′​]E[vi​vj′​]​=0=0i=j=0i=j​

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

1.）初始化

步骤一：将状态变量扩维 xa=[x′,w′,v′]′,n=nx+nw+nvx^a=[x', w', v']', n=n_x+n_w+n_vxa=[x′,w′,v′]′,n=nx​+nw​+nv​

给定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​，则

x^k−1∣k−1a=[x^k−1∣k−1′,0′,0′]′∈RnPk−1∣k−1a=[Pk−1∣k−1000Qk−1000Rk−1]\begin{aligned} &\hat{x}_{k-1|k-1}^a=[\hat{x}_{k-1|k-1}', 0', 0']'\in R^n\\ &P_{k-1|k-1}^a=\begin{bmatrix} P_{k-1|k-1}&0&0\\ 0&Q_{k-1}&0\\0&0&R_{k-1}\end{bmatrix} \end{aligned}​x^k−1∣k−1a​=[x^k−1∣k−1′​,0′,0′]′∈RnPk−1∣k−1a​=⎣⎡​Pk−1∣k−1​00​0Qk−1​0​00Rk−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∣0a=[xˉ0′,0′,0′]′∈RnPk−1∣k−1a=[P0000Q0000R0]\begin{aligned} &\hat{x}_{0|0}^a=[\bar{x}_0', 0', 0']'\in R^n\\ &P_{k-1|k-1}^a=\begin{bmatrix} P_0&0&0\\ 0&Q_0&0\\0&0&R_0\end{bmatrix} \end{aligned}​x^0∣0a​=[xˉ0′​,0′,0′]′∈RnPk−1∣k−1a​=⎣⎡​P0​00​0Q0​0​00R0​​⎦⎤​​

2. ) 状态预测

步骤二：

根据x^k−1∣k−1a,Pk−1∣k−1a\hat{x}^a_{k-1|k-1},P^a_{k-1|k-1}x^k−1∣k−1a​,Pk−1∣k−1a​，利用UT变换选取2n+12n+12n+1个sigam点和权值

Xk−1∣k−10=x^k−1∣k−1a,i=0Xk−1∣k−1i=x^k−1∣k−1a+n+λ[Pk−1∣k−1a]i,i=1,2,⋯,nXk−1∣k−1n+i=x^k−1∣k−1a−n+λ[Pk−1∣k−1a]i,i=1,2,⋯,n\begin{aligned} &\mathcal{X}^0_{k-1|k-1}=\hat{x}^a_{k-1|k-1},i=0\\ &\mathcal{X}^i_{k-1|k-1}=\hat{x}^a_{k-1|k-1}+\sqrt{n+\lambda}[\sqrt{P^a_{k-1|k-1}}]_i,i=1,2,\cdots,n\\ &\mathcal{X}^{n+i}_{k-1|k-1}=\hat{x}^a_{k-1|k-1}-\sqrt{n+\lambda}[\sqrt{P^a_{k-1|k-1}}]_i,i=1,2,\cdots,n\\ \end{aligned}​Xk−1∣k−10​=x^k−1∣k−1a​,i=0Xk−1∣k−1i​=x^k−1∣k−1a​+n+λ​[Pk−1∣k−1a​​]i​,i=1,2,⋯,nXk−1∣k−1n+i​=x^k−1∣k−1a​−n+λ​[Pk−1∣k−1a​​]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−1∣k−1i=[(Xk−1∣k−1i,x)′,(Xk−1∣k−1i,w)′,(Xk−1∣k−1i,v)′]′,i=0,1,2,⋯,2n\mathcal{X}^i_{k-1|k-1}=[(\mathcal{X}_{k-1|k-1}^{i,x})', (\mathcal{X}_{k-1|k-1}^{i,w})', (\mathcal{X}_{k-1|k-1}^{i,v})']',i=0,1,2,\cdots,2nXk−1∣k−1i​=[(Xk−1∣k−1i,x​)′,(Xk−1∣k−1i,w​)′,(Xk−1∣k−1i,v​)′]′,i=0,1,2,⋯,2n

步骤三：根据系统方程传播sigma点

Xk∣k−1i,x=f(Xk−1∣k−1i,x,Xk−1∣k−1i,w)，i=0,1,2,⋯,2n\mathcal{X}^{i,x}_{k|k-1}=f(\mathcal{X}_{k-1|k-1}^{i,x}, \mathcal{X}_{k-1|k-1}^{i,w})，i=0,1,2,\cdots,2nXk∣k−1i,x​=f(Xk−1∣k−1i,x​,Xk−1∣k−1i,w​)，i=0,1,2,⋯,2n

步骤四：状态一步预测和预测方差

xk∣k−1=∑i=02nwmXk∣k−1i,xPk∣k−1=∑i=02nwc(Xk∣k−1i,x−xk∣k−1)(Xk∣k−1i,x−xk∣k−1)′\begin{aligned} &x_{k|k-1}=\sum_{i=0}^{2n}w^m\mathcal{X}^{i,x}_{k|k-1}\\ &P_{k|k-1}=\sum_{i=0}^{2n}w^c(\mathcal{X}^{i,x}_{k|k-1}-x_{k|k-1})(\mathcal{X}^{i,x}_{k|k-1}-x_{k|k-1})' \end{aligned}​xk∣k−1​=i=0∑2n​wmXk∣k−1i,x​Pk∣k−1​=i=0∑2n​wc(Xk∣k−1i,x​−xk∣k−1​)(Xk∣k−1i,x​−xk∣k−1​)′​

3. ) 状态更新

步骤五：将Xk∣k−1i\mathcal{X}^i_{k|k-1}Xk∣k−1i​点通过量测方程传播，得到量测预测sigma点

Zk∣k−1i,x=h(Xk∣k−1i,x,Xk∣k−1i,v)，i=0,1,2,⋯,2n\mathcal{Z}^{i,x}_{k|k-1}=h(\mathcal{X}^{i,x}_{k|k-1}, \mathcal{X}^{i,v}_{k|k-1})，i=0,1,2,\cdots,2nZk∣k−1i,x​=h(Xk∣k−1i,x​,Xk∣k−1i,v​)，i=0,1,2,⋯,2n

步骤六：量测预测，量测预测误差方差（新息方差），状态与量测互协方差，增益

zk∣k−1=∑i=02nwmZk∣k−1i,xSk=∑i=02nwc(Zk∣k−1i,x−zk∣k−1)(Zk∣k−1i,x−zk∣k−1)′Ck=∑i=02nwc(Xk∣k−1i,x−xk∣k−1)(Zk∣k−1i,x−zk∣k−1)′Kk=CkSk−1\begin{aligned} &z_{k|k-1}=\sum_{i=0}^{2n}w^m\mathcal{Z}^{i,x}_{k|k-1}\\ &S_{k}=\sum_{i=0}^{2n}w^c(\mathcal{Z}^{i,x}_{k|k-1}-z_{k|k-1})(\mathcal{Z}^{i,x}_{k|k-1}-z_{k|k-1})'\\ &C_{k}=\sum_{i=0}^{2n}w^c(\mathcal{X}^{i,x}_{k|k-1}-x_{k|k-1})(\mathcal{Z}^{i,x}_{k|k-1}-z_{k|k-1})'\\ &K_k=C_{k}S_{k}^{-1} \end{aligned}​zk∣k−1​=i=0∑2n​wmZk∣k−1i,x​Sk​=i=0∑2n​wc(Zk∣k−1i,x​−zk∣k−1​)(Zk∣k−1i,x​−zk∣k−1​)′Ck​=i=0∑2n​wc(Xk∣k−1i,x​−xk∣k−1​)(Zk∣k−1i,x​−zk∣k−1​)′Kk​=Ck​Sk−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∣k​Pk∣k​​=E[xk​∣Zk]=x^k∣k−1​+Kz​(zk​−z^k∣k−1​)=cov(xk∣k​)=Pk∣k−1​−Kk​Sk​Kk′​​(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）及为带非加性噪声非线性系统的无迹卡尔曼滤波算法。

三、仿真实验

仿真部分见博客：

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

/weixin_44044161/article/details/115385918

3.1 仿真场景（三维）

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

/download/weixin_44044161/85124056

3.2 仿真场景（二维）

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

/download/weixin_44044161/85123963

3.3 二维UKF跟踪仿真结果

四、二维UKF跟踪参考代码

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

/download/weixin_44044161/85401310

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

/download/weixin_44044161/85402056

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

/download/weixin_44044161/85123963

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

/download/weixin_44044161/85124056

说明:

1.二维仿真代码也可以在上面的连接中直接下载，

2.将UKF函数保存，文件名“fun_2UKF.m”

3.将量测函数保存，文件名“measurements.m”

4. 运行下面的主函数

5. 注意将这三个文件保存在一个文件夹下

4.1 主函数

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% created by: % date: /4% 无迹卡尔曼滤波，目标跟踪 % 二维目标跟踪问题% 线性CV目标模型% 单雷达%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%clear all; close all; clc;%% initial parametern=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 noiseq_x=0.01; %m/s^2q_y=q_x;Qk=diag([q_x^2,q_y^2]); % state matrixFk= [1 T 0 00 T 0 00 0 1 T0 0 0 T]; %Gk= [ T^2/2 0T00T^2/20T ]; %%量测模型sigma_r(1)=80; sigma_b(1)=60e-3; % covariance of measurement noise (radar)Rk=diag([sigma_r(1)^2, sigma_b(1)^2]);xp=[0,0,0,0];%雷达位置

如果觉得《无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分)》对你有帮助，请点赞、收藏，并留下你的观点哦！