基于RBPF算法的动态条件高斯模型滤波器

资 源 简 介

该项目实现了针对动态条件高斯模型（DCGM）的Rao-Blackwellised粒子滤波算法。其核心思路是利用状态空间的分层结构，将系统状态划分为非线性或非高斯部分以及线性高斯部分。在算法执行过程中，通过粒子滤波（PF）对非线性状态部分进行采样估计，而对于在给定非线性部分路径下呈线性高斯特性的另一部分状态，则通过卡尔曼滤波器（KF）进行解析推导。这种边际化（Marginalization）技术有效地降低了采样空间的维度，极大地减小了重要性权重的方差，使得在较少粒子数量的情况下即可获得比标准粒子滤波更高的估

详 情 说 明

基于动态条件高斯模型(DCGM)的Rao-Blackwellised粒子滤波器系统

项目简介

本项目实现了一种先进的混合状态估计算法——Rao-Blackwellised粒子滤波器（RBPF）。该系统专门用于处理动态条件高斯模型（DCGM），这种模型具有独特的分层结构：一部分状态遵循复杂的非线性/非高斯动态演化，而另一部分状态在给定非线性部分的情况下表现为线性高斯特性。

通过应用边际化（Marginalization）技术，系统将高维状态空间的估计问题降维。非线性状态通过粒子滤波（PF）进行采样处理，而线性状态则通过卡尔曼滤波器（KF）进行解析求解。这种“采样+解析”的组合方式显著降低了重要性权重在采样过程中的方差，相比于标准粒子滤波，在更少的粒子规模下即可实现更高的收敛精度和数值稳定性。

核心功能特性

• 动态条件高斯模型仿真：内置了一个复杂的耦合系统，其中非线性状态演化包含三角函数，线性状态转移矩阵则随非线性状态动态旋转。
• 混合滤波架构：每个粒子不仅包含非线性状态的一个采样样本，还独立维护一个描述线性状态分布的卡尔曼滤波（均值向量和协方差矩阵）。
• 边际似然权重更新：利用观测预测分布的残差和创新协方差，在对数域计算似然度，有效避免数值溢出。
• 系统重采样机制：集成了系统重采样（Systematic Resampling）算法，并结合有效粒子数（N_eff）监控，仅在粒子退化严重时触发重采样，维持粒子多样性。
• 全维度状态评估：通过加权平均方式输出非线性状态估计值、线性状态均值估计以及综合后验协方差。
• 实时可视化分析：提供状态跟踪对比图、RMSE误差统计以及估计不确定度（协方差迹）的动态走势分析。

实现逻辑分析

系统的执行流程严格遵循递归贝叶斯估计框架，具体步骤如下：

1. 系统仿真实验生成

程序首先模拟生成地面真值。非线性状态 s 随正弦干扰和 atan 函数演化；线性状态 z 的转移矩阵由 s 的余弦与正弦值实时构造，模拟了一个带衰减的动态旋转系统；观测值 y 则是对线性状态 z 的加噪声直接观测。

1. 粒子群体初始化

初始时刻，系统生成指定数量的粒子。每个粒子被分配一个随机初始化的非线性采样值、线性状态的先验均值向量（0向量）以及初始协方差矩阵（单位阵的缩放）。

1. 预测与采样

在每个时间步，系统遍历所有粒子。首先利用非线性演化方程对每个粒子的状态 s 进行预测性采样；随后，利用采样后的 s 更新该粒子的系统矩阵 A，并以此执行卡尔曼滤波的预测步，得到线性状态的预测均值和预测协方差。

1. 权重更新

这是 RBPF 的核心步骤。系统并不直接按观测值对 s 评估，而是通过边际似然进行更新。即计算观测值 y 与线性预测值之间的残差，并结合卡尔曼滤波中的创新协方差矩阵，计算该粒子在当前观测下的概率似然，从而更新粒子的重要性权重。

1. 卡尔曼修正

在获得观测值后，每个粒子利用计算出的增益 K 对其内部维护的线性状态均值和协方差进行修正，完成条件线性部分的后验更新。

1. 重采样与状态输出

系统计算有效粒子数，若低于设定的阈值（粒子总数的一半），则调用系统重采样算法，根据权重比例重新分配粒子，消除权重极低的样本。最后，系统通过所有粒子的加权平均计算出当前的最优估计状态。

关键算法说明

• Rao-Blackwellization (边际化)：这是本项目算法的核心。它利用了全概率公式：p(s, z | y) = p(z | s, y) * p(s | y)。由于 p(z | s, y) 是高斯的，可以通过卡尔曼滤波解析处理，粒子滤波仅需负责估计边际分布 p(s | y)。
• 对数域似然计算：在权重更新中，通过计算多元正态分布的对数概率再取指数，增强了处理极小权重时的数值健壮性。
• 系统重采样 (Systematic Resampling)：相比于简单随机重采样，该方法通过在单位区间内生成等间距的采样点，确保了权重较大的粒子能以更稳定的比例被选取，减少了随机采样带来的噪声。
• 不确定性评估：程序不仅输出了状态点估计，还结合了每个粒子的内部协方差和粒子群体的样本方差，计算出总体的后验协方差迹（Trace of Covariance），反映了滤波器对估计结果的信心。

系统要求与使用方法

• 系统要求：

* 环境：MATLAB R2016b 或更高版本。 * 工具箱：无需特殊工具箱，核心算法基于基础数学矩阵运算。

• 使用方法：

1. 打开 MATLAB 软件。 2. 将包含程序代码的文件夹设置为当前工作路径。 3. 在命令行窗口输入入口函数名称。 4. 程序将自动运行 T=60 步的滤波演示。 5. 运行结束后，系统会弹出两个图形窗口：一个展示状态跟踪曲线，另一个展示误差分析与不确定度边界，同时命令行会打印非线性状态与线性状态的平均 RMSE。

应用领域

该系统实现的算法适用于以下场景：

• SLAM (即时定位与地图构建)：其中机器人位姿是非线性部分，地图特征点位置是线性高斯部分。
• 机动目标跟踪：目标的位置演化与复杂的航向角（非线性）相耦合。
• 通信信号处理：在多径环境下的非线性参数估计与线性信号还原。
• 金融工程：包含随机波动率（非线性）与基础资产收益（线性）的动态模型分析。