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

Posted 2023-01-14 脑壳二

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

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

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

仿真部分见博客：
[无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)
链接: https://blog.csdn.net/weixin_44044161/article/details/115390660.

作者:[e-mail]
备注：
无迹卡尔曼滤波算法；无迹滤波；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

无迹滤波思考：

CKF和UKF 总结：
当取$\kappa =0$时， CKF 和 UKF 的估计精度相同，但鉴于 CKF 采样点少，实时性
比 UKF 好，故应选用 CKF 滤波算法；
当$n\le 2$时即低维非线性系统， UKF 的估计精度高于 CKF，应选用 UKF 滤波
算法；
当$n=2$时的非线性系统， UKF 及 CKF 的估计精度相同，但 CKF 的实时性更
好，应选用 CKF 滤波算法；
当 $n\ge 3$时即高维非线性系统， CKF 的估计精度高于 UKF，应选用 CKF 滤波算法。

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

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

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}$$
w 0 m = λ n + λ w 0 c = λ n + λ + ( 1 − α 2 + β ) w i m = w i c = 1 2 n + 2 λ , i = 1 , 2