扩展卡尔曼滤波EKF—目标跟踪中的应用(算法部分)

Posted 2023-01-14 脑壳二

扩展卡尔曼滤波EKF—目标跟踪中的应用(算法部分)

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

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

仿真部分见博客：
扩展卡尔曼滤波EKF—目标跟踪中的应用(仿真部分)

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

作者:[e-mail]
备注：
扩展卡尔曼滤波算法；
目标跟踪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

扩展卡尔曼滤波—及其在目标跟踪中的应用

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

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

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 状态一步预测及预测误差协方差阵为
x ^ k ∣ k − 1 = E [ x k ∣ Z k − 1 ] ≈ E [ f k − 1 ( x ^ k −