基于扩展卡尔曼改进的粒子滤波算法

资 源 简 介

本项目主要实现了一种结合扩展卡尔曼滤波（EKF）与粒子滤波（PF）的高级状态估计算法。该算法的核心思想是利用EKF生成的后验分布来作为粒子滤波的建议分布（Proposal Distribution），而不是简单地使用状态转移先验。通过引入最新的观测信息，EKF-PF能够使粒子分布更接近真实的后验概率密度函数，从而有效缓解传统粒子滤波中常见的“粒子退化”问题，并显著减少此时所需的粒子数量。项目详细构建了非线性动态系统的预测与更新流程，包括雅可比矩阵的线性化计算、重要性权重的更新、以及防止样本贫化的重采样（Resampling）机制。该代码适用于解决强非线性、非高斯环境下的复杂状态估计问题，如机动目标跟踪、机器人SLAM（同步定位与地图构建）及导航制导等场景，能够输出高精度的状态估计结果并对比分析算法性能。

详 情 说 明

EKF-PF 基于扩展卡尔曼的粒子滤波算法项目

项目简介

本项目实现了一种高级的状态估计算法，即 EKF-PF（Extended Kalman Filter - Particle Filter）。该算法将扩展卡尔曼滤波（EKF）嵌入到粒子滤波（PF）的框架中，核心在于利用 EKF 生成的后验分布作为粒子滤波的建议分布（Proposal Distribution）。

传统的粒子滤波通常使用状态转移先验作为建议分布，这在观测精度高或状态变化快时容易导致“粒子退化”现象，即大部分粒子权重接近于零。本项目通过引入最新观测值并在每个粒子上运行 EKF 更新，构建了更接近真实后验的建议分布，从而显著提高了估计精度，并能在使用较少粒子（如代码中仅设为 100 个）的情况下保持算法的鲁棒性。

功能特性

• 混合滤波架构：结合了 EKF 处理非线性的局部线性化能力和 PF 处理非高斯分布的全局蒙特卡洛采样能力。
• 高效建议分布：不同于标准 SIR 粒子滤波，本算法利用 EKF 更新后的高斯分布 $N(mu_{ekk}, Sigma_{ekk})$ 进行粒子采样，使粒子更集中于高似然区域。
• 非线性观测处理：基于距离（Range）和方位角（Bearing）的主动观测模型，模拟雷达或声纳跟踪场景。
• 抗粒子退化：通过改进建议分布和系统重采样机制，有效缓解样本贫化问题。
• 全流程仿真：包含真实轨迹生成、含噪数据模拟、状态估计及误差分析闭环。

算法实现细节

本项目的主程序脚本通过以下逻辑流程完成了对机动目标的跟踪任务：

1. 系统建模与初始化

• 状态定义：系统状态向量包含二维平面上的位置和速度信息 $[x, v_x, y, v_y]^T$。
• 运动模型：采用匀速模型（CV），状态转移矩阵 $F$ 基于采样时间间隔 $dt$ 构建。过程噪声 $Q$ 考虑了加速度扰动。
• 观测模型：非线性观测函数 $h(x)$ 输出目标相对于原点的距离 $r$ 和方位角 $theta$。观测噪声 $R$ 为对角矩阵。
• 粒子集初始化：在初始估计状态周围按高斯分布生成粒子群，且每个粒子不仅维护自身的状态向量，还独立维护一个协方差矩阵 $P$，用于后续的 EKF 更新。

2. EKF-PF 核心循环

算法在每一时间步针对每一个粒子执行以下操作：

• EKF 预测与更新（生成建议分布）：

1. 利用运动模型预测粒子的先验状态和协方差。 2. 雅可比线性化：计算观测函数相对于状态的雅可比矩阵 $H$，用于处理非线性观测。 3. 计算卡尔曼增益 $K$，并利用当前观测值 $z_{curr}$ 更新粒子的均值 $mu_{ekk}$ 和协方差 $Sigma_{ekk}$。 4. 角度处理：在计算观测残差时，代码包含了角度归一化逻辑，确保角度差值保持在 $[-pi, pi]$ 区间内。 5. 采样：从 EKF 更新后的高斯分布 $N(mu_{ekk}, Sigma_{ekk})$ 中采样生成新的粒子 $x_{new}$。

• 重要性权重计算：

* 权重的更新公式基于：$w propto frac{p(z|x) p(x|x_{prev})}{q(x|x_{prev}, z)}$。 * 代码分别计算了观测似然概率（Likelihood）、状态转移概率（Transition Prior）和建议分布概率（Proposal Probability）。 * 为防止数值下溢，所有概率计算均在对数域（Log Domain）进行，最后再转换回线性域并归一化。

3. 重采样机制

• 计算有效粒子数 $N_{eff}$（Effective Sample Size）。
• 设定阈值（如粒子总数的一半），当 $N_{eff}$ 低于阈值时触发重采样。
• 采用系统重采样（Systematic Resampling）算法，根据累积权重分布（CDF）复制高权重粒子并淘汰低权重粒子，同时对应的协方差矩阵 $P$ 也被重采样。

4. 结果分析与可视化

仿真结束后，程序会自动生成三组图表以评估算法性能：

1. 2D 轨迹对比图：在笛卡尔坐标系下绘制真实轨迹、EKF-PF 估计轨迹以及转换后的含噪观测点，直观展示跟踪效果。
2. 状态误差曲线：分别绘制 X 位置、X 速度、Y 位置、Y 速度随时间的误差曲线。
3. RMSE 分析：计算并绘制位置（Position）和速度（Velocity）的均方根误差随时间的演化，用于量化收敛性能。

使用方法

1. 确保计算机上安装了 MATLAB。
2. 将项目代码保存为 .m 文件。
3. 直接运行该脚本。
4. 程序运行期间，工作区将存储历史状态数据，运行结束后将弹出三个性能分析图窗。

系统要求

• 软件：MATLAB (推荐 R2016b 及以上版本，需包含基础工具箱)。
• 硬件：标准 PC 即可，由于代码已经过向量化优化且粒子数较少（100个），计算开销极低。