粒子滤波算法原理与非线性系统仿真源码

资 源 简 介

本项目旨在深入探究和演示粒子滤波（Particle Filter）的核心原理与实现流程，特别针对非线性、非高斯环境下的状态估计问题。系统将构建一个经典的动态系统仿真环境（如一维非静止增长模型或二维目标跟踪模型），完整实现序列重要性采样（SIS）和序列重要性重采样（SIR/Bootstrap）算法。具体功能描述如下：1. 状态空间建模：在MATLAB中定义非线性状态方程和非线性观测方程，并添加高斯或非高斯白噪声以模拟真实环境；2. 算法核心实现：详细编写粒子滤波的四个关键步骤代码，包括粒子群的初始化（从先验分布采样）、预测（通过状态转移方程传播粒子）、更新（根据观测值计算粒子权重/似然度）以及重采样（消除低权重粒子，复制高权重粒子，防止粒子退化）；3. 动态可视化：利用MATLAB绘图功能，实时动态显示真实状态轨迹、观测值散点、滤波估计轨迹以及粒子群的分布变化情况，通过直观的视觉效果展示粒子如何“聚集”在真实状态附近；4. 性能评估与对比：计算并输出均方根误差（RMSE）指标，绘制误差收敛曲线，并可扩展加入扩展卡尔曼滤波（EKF）的对比仿真，以突显粒子滤波在处理强非线性问题上的优势。项目代码将包含详细注释，解释每一步的贝叶斯推断逻辑，辅助理论学习。

详 情 说 明

粒子滤波经典算法仿真演示系统

项目介绍

本项目是一个基于MATLAB开发的动态系统状态估计仿真平台，旨在直观演示和验证粒子滤波（Particle Filter, PF）核心算法在处理非线性、非高斯问题上的优势。仿真场景构建了一个经典的二维雷达目标跟踪环境，目标在平面内做近似匀速运动，而雷达通过非线性观测方程（距离和方位角）对目标进行探测。

系统不仅完整实现了粒子滤波的序列重要性采样（SIS）与重采样（SIR）流程，还同步实现了扩展卡尔曼滤波（EKF）作为对比基准，配合实时的动态可视化界面，帮助用户深入理解贝叶斯推断过程及粒子群分布演变规律。

核心功能特性

1. 二维非线性系统建模

• 状态模型：构建了包含位置（x, y）和速度（vx, vy）的四维状态空间，采用常速度（CV）运动模型驱动状态转移，并引入高斯白噪声模拟过程不确定性。
• 观测模型：实现了雷达观测机制，输出目标距离（Range）和方位角（Bearing）两个非线性量测值，并叠加了测量噪声。

2. 粒子滤波核心算法实现

• 蒙特卡洛采样：系统初始化时从先验分布中生成高斯分布的随机粒子群。
• 预测（Prediction）：基于状态转移矩阵和过程噪声，独立传播每个粒子的状态。
• 更新（Update）：计算每个粒子的观测似然度。为了防止数值下溢，算法采用了对数权重（Log-weights）计算策略，再转换回归一化权重。
• 重采样（Resampling）：引入有效粒子数（N_eff）监控机制。当粒子退化严重时，执行系统重采样（Systematic Resampling），淘汰低权重粒子并复制高权重粒子。

3. 多算法对比分析

• EKF 对照组：内置扩展卡尔曼滤波算法，通过计算雅可比矩阵（Jacobian）线性化观测方程，作为传统非线性滤波方法的性能参照。
• 误差量化：实时计算并记录两种算法在位置和速度上的估计误差。

4. 沉浸式动态可视化

• 轨迹跟踪图：同屏显示真实轨迹、含噪观测点（坐标转换后）、EKF估计轨迹、PF加权均值轨迹以及粒子群的实时散布情况。
• 权重分布图：通过直方图实时展示当前粒子权重的分布形态。
• 误差收敛图：动态绘制时间步与位置误差的关系曲线，直观对比收敛速度与稳态精度。

系统要求

• MATLAB R2016b 或更高版本
• 无需额外工具箱（主要使用基础矩阵运算功能）

使用方法

1. 确保MATLAB当前路径包含项目文件夹。
2. 直接运行主脚本。
3. 系统将自动弹出一个可视化窗口，动态播放50秒的仿真过程。
4. 仿真结束后，控制台将输出PF与EKF的RMSE统计结果，并弹出包含位置与速度详细误差曲线的分析图表。

---

算法实现细节与逻辑解析

本项目代码严格遵循贝叶斯滤波框架，以下是对其内部实现逻辑的详细解析：

1. 初始化阶段

系统首先设定仿真参数，包括总时长（50秒）、采样间隔（0.1秒）以及粒子总数（1000个）。

• 真实数据生成：利用物理运动方程迭代生成真实的状态轨迹，并通过非线性函数 $h(x) = [sqrt{x^2+y^2}, arctan(y/x)]^T$ 生成观测数据。
• 粒子群初始化：在初始真实状态附近添加一定的随机扰动，生成初始粒子集合，权重初始化为均匀分布 $1/N$。
• EKF初始化：设定初始协方差矩阵 $P$ 和带有初始偏差的状态估计值。

2. 仿真主循环逻辑

循环遍历每一个时间步，依次执行以下操作：

#### 扩展卡尔曼滤波 (EKF) 模块

1. 预测：利用状态转移矩阵 $F$ 预测下一时刻状态和协方差。
2. 线性化：计算观测方程在预测状态处的雅可比矩阵 $H$，处理除零保护。
3. 更新：计算卡尔曼增益，利用观测残差（Innovation）修正状态估计。代码中特别处理了角度残差的归一化，确保角度差在 $[-pi, pi]$ 范围内。

#### 粒子滤波 (PF) 模块

1. 粒子传播：对所有粒子应用状态转移方程，并叠加随机过程噪声，模拟系统的动态演化。
2. 权重计算：

* 针对当前时刻的观测向量，计算每个粒子对应的预测观测值（距离和角度）。 * 计算观测残差，并基于假设的高斯观测噪声模型计算对数似然度。 * 使用 exp(log_w - max_log_w) 技巧将对数权重转换为线性权重，有效避免了计算机浮点数精度下溢的问题。 * 对权重进行归一化处理。

1. 状态估计输出：计算粒子群的加权均值作为当前的系统状态估计值。
2. 自适应重采样：

* 计算有效粒子数 $N_{eff} = 1 / sum w_i^2$。 * 若 $N_{eff}$ 低于阈值（粒子总数的一半），则触发重采样过程，重置权重并重新分布粒子，防止粒子贫化（Degradation）。

3. 可视化与性能评估

• 动态绘图：为了保证仿真运行速度，代码设置了每5个时间步更新一次图形界面（drawnow limitrate）。图中将观测数据从极坐标转换回直角坐标以便于直观对比。
• 结果统计：仿真结束后，代码基于全过程数据计算均方根误差（RMSE），分别统计PF和EKF在位置（X, Y综合）和速度上的精度表现，并在控制台打印对比报表。

4. 辅助功能

• 角度归一化：在处理方位角观测数据时，包含逻辑确保角度差值计算的正确性（处理跨越 $pmpi$ 的不连续性）。