粒子滤波算法

粒子滤波（PF：Particle Filter）的思想基于蒙特卡洛方法（Monte Carlo methods），它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法（Sequential Importance Sampling）。

简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分布的过程。这里的样本即指粒子，当样本数量时可以逼近任何形式的概率密度分布。尽管算法中的概率分布只是真实分布的一种近似，但由于非参数化的特点，它摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约，能表达比高斯模型更广泛的分布，也对变量参数的非线性特性有更强的建模能力。因此，粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布，可以用于解决 SLAM 问题。

参考

从贝叶斯滤波到粒子滤波 | 朝花夕拾 (shipengx.com)

车辆（十）——粒子滤波 - 知乎 (zhihu.com)

粒子滤波算法理解及实现-CSDN 博客

粒子滤波（Particle filter）算法简介及 MATLAB 实现_粒子滤波算法-CSDN 博客

粒子滤波（Particle Filtering）简介_饕子的博客-CSDN 博客

核心原理

粒子滤波基于贝叶斯滤波框架，用一组带权重的随机粒子来近似后验概率分布。

状态方程：

观测方程：

后验概率近似（蒙特卡洛采样）：

权重递推（重要性采样）：

最小方差估计（加权平均）：

算法流程

初始阶段

在开始滤波过程之前，需要对粒子集合进行初始化。通常可以采用在给定初值处均匀分布或其他分布形式来生成一组粒子，表示系统的初始状态。（本例中采用高斯分布初始化粒子）

预测阶段

根据系统模型，对每个粒子进行状态转移预测，以得到下一时刻的粒子。

更新阶段

根据系统模型得到预测值，根据测量结果获得观测数据，以观测值为均值的高斯分布进行每个粒子权重的计算，并进行归一化处理。权重表示了每个粒子与观测数据的一致程度。

重采样阶段

根据粒子的权重，进行重采样操作（本例中采用独立随机采样，类似轮盘赌），增加高权重的粒子并减少低权重的粒子。重采样操作能够增加高权重粒子的数量，提高估计的准确性，可以通过设置阈值以决定是否进行重采样。

滤波输出

每一次迭代都会根据系统模型和观测模型，进行状态的预测和更新，从而逐渐减小估计误差。

算法思想

均值估计

粒子滤波利用粒子集合的均值来作为滤波器的估计值，前提是粒子集合的分布很好地"覆盖"真实值。

权重计算

根据权重大小能实现"优质"粒子的大量复制，对"劣质"粒子实现淘汰制，是粒子滤波算法的核心。

MATLAB 实现

运行环境：Matlab 2018a

参数初始化

设定噪声协方差、粒子数和仿真步长，将初始粒子集建模为真实初值 附近的高斯分布。

% SIR (Sampling Importance Resampling) 粒子滤波
clear all; close all; clc

% ---- 系统参数 ----
x_init = 0.1;           % 初始真实值
x_N = 1;                % 模型噪声协方差 Q
x_R = 1;                % 观测噪声协方差 R
T = 75;                 % 仿真步数
N = 1000;               % 粒子数（越大精度越高，计算量越大）

% ---- 粒子初始化（高斯分布） ----
V = 2;                  % 初始粒子方差
X_ParticleModel = zeros(N, T);
X_Particle      = zeros(N, T);
Z_ParticleModel = zeros(N, T);
Particle_Probility     = zeros(N, T);
Particle_NormProbility = zeros(N, T);

X_Value    = zeros(1, T);   % 真实状态序列
X_EstValue = zeros(1, T);   % 滤波估计序列
Z_Value    = zeros(1, T);   % 观测序列

% 在初值附近生成 N 个粒子
for i = 1:N
    X_ParticleModel(i, 1) = x_init + sqrt(V) * randn;
end
X_Value(1, 1)    = x_init;
X_EstValue(1, 1) = x_init;

主循环（预测 → 更新 → 重采样）

每步依次执行：预测真实状态、生成观测值、粒子预测、计算似然权重、归一化、重采样、状态估计。

for t = 2:T
    % ---- 真实系统演化（实际环境中不可知） ----
    X_Value(1, t) = 0.5 * X_Value(1, t-1) ...
        + 25 * X_Value(1, t-1) / (1 + X_Value(1, t-1)^2) ...
        + 8 * cos(1.2*(t-1)) + sqrt(x_N) * randn;
    % ---- 观测生成 ----
    Z_Value(1, t) = X_Value(1, t)^2 / 20 + sqrt(x_R) * randn;

    for i = 1:N
        % ① 预测：每个粒子按模型状态转移
        X_ParticleModel(i, t) = 0.5 * X_ParticleModel(i, t-1) ...
            + 25 * X_ParticleModel(i, t-1) / (1 + X_ParticleModel(i, t-1)^2) ...
            + 8 * cos(1.2*(t-1)) + sqrt(x_N) * randn;
        % ② 更新：计算粒子观测值与似然
        Z_ParticleModel(i, t) = X_ParticleModel(i, t)^2 / 20;
        % ③ 权重：以高斯似然计算
        Particle_Probility(i, t) = (1 / sqrt(2*pi*x_R)) ...
            * exp(-(Z_Value(1,t) - Z_ParticleModel(i,t))^2 / (2*x_R));
    end
    % ④ 权重归一化
    Particle_NormProbility(:, t) = Particle_Probility(:, t) ./ sum(Particle_Probility(:, t));

    % ⑤ 重采样（轮盘赌 / 独立随机采样）
    for i = 1:N
        X_Particle(i, t) = X_ParticleModel(...
            find(rand <= cumsum(Particle_NormProbility(:, t)), 1), t);
    end
    % ⑥ 状态估计：重采样后权重均为 1/N，取粒子均值
    X_EstValue(1, t) = mean(X_Particle(:, t));
end

结果可视化

t = 1:T;
figure(1);
plot(t, X_Value(1,:), '.-b', t, X_EstValue(1,:), '-.r', 'linewidth', 3);
set(gca, 'FontSize', 12);
set(gcf, 'Color', 'White');
xlabel('Step'); ylabel('Value');
legend('真实值', '估计值');