贝叶斯滤波与平滑全书习题及算法实现项目

资 源 简 介

该项目是对著名学术著作《Bayesian Filtering and Smoothing》中所有课后习题和算法实例的完整MATLAB复现。其核心功能在于构建一套规范化的动态系统估计框架，通过编写模块化的函数来实现概率状态估计理论。程序覆盖了线性系统的卡尔曼滤波（KF）及其预测与更新步骤的完整循环，同时针对非线性系统实现了扩展卡尔曼滤波（EKF）的一阶泰勒展开逼近，以及无迹卡尔曼滤波（UKF）的Sigma点采样技术。此外，项目详细实现了离线数据处理中至关重要的平滑算法，包括线性系统的Rauch-Tung-S

详 情 说 明

项目名称：贝叶斯滤波与平滑算法（MATLAB实现）

项目介绍

本项目是对著名学术著作《Bayesian Filtering and Smoothing》（Simo Särkkä 著）中核心算法的系统化MATLAB复现。项目以状态空间模型为基础，构建了一个标准化的动态系统估计框架。其核心目标是解决在含噪声环境下，如何通过观测数据递归地估计时变系统的内部状态。通过模块化的代码设计，项目完整实现了从线性卡尔曼滤波到强非线性粒子滤波及离线平滑的一系列数学工具。

功能特性

1. 多算法集成：在一个统一的框架下集成了扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）和粒子滤波（PF）。
2. 平滑处理能力：实现了针对非线性系统的扩展拉auch-Tung-Striebel平滑器（ERTSS）和无迹平滑器（URTSS），提供比实时滤波更精确的后续离线分析。
3. 非线性建模：专门设计了极坐标观测模型（距离与角度），模拟雷达追踪等真实物理场景。
4. 自动化评估：包含自动生成仿真轨迹、添加随机噪声、计算均方根误差（RMSE）以及多维度可视化对比的功能。

运行环境

• MATLAB R2016b 或更高版本。
• 需要安装 Statistics and Machine Learning Toolbox（用于调用 mvnrnd 等概率分布函数）。

使用方法

1. 在 MATLAB 环境中打开项目所在文件夹。
2. 直接运行文件即可启动仿真。
3. 程序将自动执行以下流程：

- 初始化匀速运动学模型参数。 - 生成带有噪声的真实轨迹与极坐标观测数据。 - 顺序执行各个滤波与平滑算法。 - 弹出可视化窗口，展示轨迹对比图、瞬时误差曲线及算法性能统计柱状图。 - 在命令行输出各项算法的 RMSE 精度对比。

算法实现细节

1. 系统模型定义

项目采用四维状态向量 [x坐标, y坐标, x速度, y速度] 的匀速运动（CV）模型作为动态过程。观测模型为非线性模型，将笛卡尔坐标下的位置转换为传感器可接收的距离（rho）和方位角（phi）。

2. 扩展卡尔曼滤波 (EKF)

在更新步骤中，程序通过手动计算观测函数的雅可比矩阵（Jacobian），将非线性观测模型在线性化点附近进行一阶泰勒展开，从而利用标准卡尔曼公式进行后验更新。

3. 无迹卡尔曼滤波 (UKF)

采用无迹变换（Unscented Transform）技术。程序基于状态维数生成 2n+1 个 Sigma 点，通过非线性函数直接传递这些采样点，利用加权统计的方法计算均值和协方差。这种方法避免了雅可比矩阵的推导，对非线性系统的处理精度通常优于 EKF。

4. 粒子滤波 (PF)

采用基于序贯重要性采样（SIS）的粒子滤波算法。

• 权重计算：利用高斯似然函数根据观测残差计算每个粒子的权重。
• 重采样策略：引入了系统重采样（Systematic Resampling）机制。当有效粒子数（Effective Particle Size）低于设定的阈值时触发重采样，以缓解粒子退化问题。

5. 状态平滑器 (ERTSS & URTSS)

项目实现了双向处理框架。在完成前向滤波后，程序会利用保存的状态估计和协方差进行后向递归。对于本项目的线性动态系统，ERTSS 和 URTSS 通过计算增益矩阵 Ck，利用未来时刻的平滑估计值来修正当前时刻的状态，显著降低了估计滞后和噪声干扰。

评估与逻辑构成

• generate_data：负责生成符合状态转移概率的真实路径，并根据观测噪声协方差 R 叠加高斯噪声。
• measurement_model：核心算子，同时输出非线性映射值及其对应的雅可比矩阵 H，支持 EKF 的线性化需求。
• 误差分析：程序计算了每个时间步长的欧氏距离误差，并计算全过程的均方根误差（RMSE），作为评价算法优劣的硬指标。
• 可视化模块：

- 轨迹图：直观展示真实轨迹、带噪观测点与各算法滤波后的平滑轨迹。 - 动态误差图：展示不同算法随时间变化的精度波动情况。 - 统计图：通过柱状图清晰对比 EKF、UKF、PF 与平滑算法的整体精度差异。