粒子滤波（直接实操版）

2301_80118595

于 2025-02-27 12:41:39 发布

阅读量1.1k

点赞数 34

文章标签：算法

本文链接：https://blog.csdn.net/2301_80118595/article/details/145900968

版权

粒子滤波

前言

上一期的卡尔曼滤波算法大家点赞比较多，我也觉得这种分享学习经验的博客很有意义，既记录了自己的学习轨迹，也帮助大家更好地理解滤波算法。因此，这一期给大家带来了粒子滤波的基本原理与源代码，希望大家喜欢。

1. 算法概述

粒子滤波（Particle Filter）是一种基于贝叶斯理论的非线性、非高斯状态估计方法。它通过一组随机样本（称为粒子）来近似表示概率分布，适用于动态系统的状态估计。粒子滤波算法广泛应用于目标跟踪、机器人导航（可能有点过时）、信号处理等领域。总而言之，非线性处理是其重要特性。

2. 算法原理

2.1 贝叶斯滤波框架

粒子滤波算法基于贝叶斯滤波框架，其核心思想是通过预测和更新两个步骤递推计算当前状态的可信度。具体来说：

预测步骤：根据上一时刻的状态和系统状态转移函数，预测当前时刻的状态。
更新步骤：根据观测信息和重要性密度函数，更新粒子的权重，并通过重采样平衡粒子的权重。

2.2 马尔可夫假设

粒子滤波算法假设状态转移和观测模型满足马尔可夫假设，即当前状态只与前一时刻的状态有关，而与更早的状态无关。

2.3 蒙特卡洛方法

粒子滤波算法采用蒙特卡洛方法，通过采样粒子并计算其权重，再根据权重求和加权得到当前状态的期望值，即滤波后的状态估计。

3. 算法步骤

3.1 初始化

选择初始粒子：从先验分布中抽取粒子，并为每个粒子设置初始权重为 $1/ N$ ，其中 $N$ 是粒子总数。
初始化状态：将粒子分布在一个初始状态空间中。

3.2 预测步骤

状态转移：根据系统状态转移函数 $f(x_k∣y_{1:k−1})$ ，预测下一时刻的状态 $x_{k+1}$ 。
更新权重：根据预测后的状态，更新每个粒子的权重。

3.3 更新步骤

重要性采样：根据观测模型 $p(y_k∣x_k)$ ，计算每个粒子的权重。
归一化权重：将所有粒子的权重进行归一化处理。

3.4 重采样

判断是否重采样：如果有效粒子数量小于阈值，则进行重采样。
重采样过程：根据粒子的权重进行重采样，生成新的粒子集合。

3.5 循环迭代

重复上述预测、更新和重采样步骤，直到达到指定的迭代次数或满足停止条件。

4、实例代码

% 初始化参数

NP = 100; % 粒子数量

T = 50; % 迭代次数

x_true = zeros(1, T); % 真实状态初始化

x_est = zeros(1, T); % 估计状态初始化

particles = randn(NP, T); % 粒子位置初始化

weights = ones(NP, 1) / NP; % 粒子权重初始化

% 模拟真实状态和测量值

for t = 2:T

x_true(t) = 0.5 * x_true(t-1) + 0.1 * randn; % 真实状态转移

z = x_true(t) + 0.1 * randn; % 测量值

% 预测阶段

particles(:, t) = 0.5 * particles(:, t-1) + 0.1 * randn(NP, 1);

% 校正阶段

weights = exp(-(particles(:, t) - z).^2 / (2 * 0.1^2));

weights = weights / sum(weights);

% 重采样阶段

if t < T

indices = datasample(1:NP, NP, 'Weights', weights);

particles(:, t+1) = particles(indices, t);

end

% 估计状态

x_est(t) = sum(particles(:, t) .* weights);

end

% 可视化结果

figure;

subplot(2, 1, 1);

plot(1:T, x_true, 'b', 'LineWidth', 2);

hold on;

plot(1:T, x_est, 'r--', 'LineWidth', 2);

scatter(1:T, particles', 'filled');

legend('真实状态', '估计状态', '粒子');

xlabel('时间');

ylabel('状态');

title('粒子滤波结果');

grid on;

subplot(2, 1, 2);

bar(weights);

xlabel('粒子索引');

ylabel('权重');

title('粒子权重');

grid on;