简介:一份粒子滤波MATLAB实现,面向信号处理与目标跟踪方向的初学者和研究者,用于解决非线性、非高斯环境下的状态估计与去噪问题。压缩包共1个文件,为后缀.m的源码文件,大小约4KB,单文件便于快速阅读和直接运行。粒子滤波作为贝叶斯滤波的蒙特卡洛近似,通过一组加权随机样本逼近后验分布,能应对传统卡尔曼滤波难以处理的强非线性场景。程序完整呈现“初始化—预测—评估—重采样—更新”流程,读者可逐步梳理粒子退化、权重更新与重采样策略等关键概念,并观察算法在信号去噪和动态目标跟踪中的实际效果。已有407人学习下载,适合配合理论教材对照实践,也可作为改进采样策略或扩展多目标跟踪的起点。整体代码量精简,门槛不高,但涉及原理较深,建议具备一定MATLAB基础后上手。
1. 为什么在MATLAB里用粒子滤波做去噪与目标跟踪
手头这份 particlefilter.rar 并不像常见的 MATLAB 工具箱那样提供一个封装好的去噪函数,解压后就是“粒子滤波.m”。看过脚本的人第一感受往往是不适应:没有 smoothdata 那种一行调用,而是一整段粒子生成、权重更新、重采样的循环。这正是粒子滤波的价值所在:当观测噪声是非高斯、状态方程是强非线性时,卡尔曼滤波的线性化假设会失效,用一簇随机粒子逼近后验分布反而能同时完成信号去噪和目标跟踪。对正在做 MATLAB 信号处理、机器人定位或者视觉跟踪的工程师来说,与其在 Simulink 里拖模块,不如先把这一个 .m 文件的流程吃透,再去决定要不要换工具箱函数。
2. 粒子滤波的递推估计与MATLAB状态空间建模
2.1 贝叶斯递推与“粒子”这个近似手段
粒子滤波解决的并不是“如何让曲线更平滑”这种后处理问题,而是在贝叶斯滤波框架下递推估计系统状态。每个时刻要做两件事:预测和更新。预测是利用状态转移模型把上一时刻的后验推到当前时刻,更新则是用最新的量测值去修正预测结果。卡尔曼滤波能在线性高斯场景下用解析公式完成这两步,可一旦状态转移或者量测模型出现强非线性,比如自动驾驶中雷达量测与目标状态的极坐标转换,直接用线性化会丢掉高次项信息。
粒子滤波换了一条路:不求解后验的闭式表达式,而是用 N 个带权重的样本去离散近似它。每个样本就是“粒子”,代表一个可能的状态。权重大小代表这个状态与真实量测的匹配程度。后验分布被写成这些粒子的加权和,均值、方差都能从样本统计中得到。这个思路很直接,代价是 N 个粒子需要同时推进,并且权重会随时间集中到少数粒子上,也就是教科书上常说的粒子退化。
2.2 MATLAB中定义状态方程与量测方程
去噪任务里最常见也最容易验证的模型是一阶自回归过程。真实信号为 x_k,它只依赖上一时刻的 x_{k-1},叠加一个过程噪声 v_k;量测 z_k 是 x_k 加观测噪声 w_k。用 MATLAB 实现时不需要符号推导,直接把模型参数放到数组里。我一般会先固定随机种子,因为粒子滤波的随机性很强,种子不固定很难比较不同参数之间的差异。
rng(2026); % 固定随机种子,结果可复现
T = 200; % 仿真/去噪时长
N = 500; % 粒子数
a = 0.98; % AR(1)状态转移系数
Q_sim = 0.1^2; % 过程噪声方差
R_sim = 0.3^2; % 观测噪声方差,用于仿真观测
x_true = zeros(T, 1);
z = zeros(T, 1);
x_true(1) = 0.5;
z(1) = x_true(1) + sqrt(R_sim) * randn;
for k = 2:T
x_true(k) = a * x_true(k-1) + sqrt(Q_sim) * randn;
z(k) = x_true(k) + sqrt(R_sim) * randn;
end
这段代码生成了用于仿真的真实状态和观测序列。 a=0.98 意味着相邻时刻信号相关性很高,曲线不会突然跳变,适合用来对比滤波后的平滑度。 Q_sim 控制过程噪声,是状态预测中的不确定性来源; R_sim 控制量测噪声,在后面的权重更新中会直接决定粒子的似然峰有多尖。实际使用粒子滤波.m 时,如果只有量测数据而没有真实状态,就把 x_true 这部分的生成逻辑删掉,只保留 z。
| 符号 | 物理含义 | 本例取值 | 对滤波结果的影响 |
|---|---|---|---|
| T | 时间步数 | 200 | 步数太少看不到收敛趋势 |
| N | 粒子数 | 500 | 精度上限与计算量的平衡点 |
| a | AR(1) 系数 | 0.98 | 越接近1,信号变化越平缓,滤波输出越平滑 |
| Q_sim | 过程噪声方差 | 0.01 | 过小会让粒子预测过自信,过大则抖动明显 |
| R_sim | 量测噪声方差 | 0.09 | 权重似然的锐度,R过大时粒子权重区分度低 |
这个建模方式和卡尔曼滤波的显著区别是,噪声分布没有写死成高斯。即使把 sqrt(Q_sim)*randn 改成 0.1 * (rand - 0.5) 的均匀分布,粒子滤波的循环依然成立,只需要在似然计算时替换为对应的均匀分布表达式。这是 MATLAB 里验证算法是否真的解决非线性非高斯问题的最快路径。
3. MATLAB粒子滤波去噪循环:初始化、权重更新与重采样
3.1 初始化粒子集与第一轮预测
在 粒子滤波.m 这类脚本里,最容易被跳过的是初始化。常见初始化方式是把第一帧量测作为粒子中心,再加上与量测噪声同量级的随机扰动。比如 repmat(z(1), N, 1) + sqrt(R_sim) * randn(N, 1) 。不要把所有粒子都初始化为同一个值,否则第一轮更新后权重几乎等于 1/N,状态估计直接落到量测上,退化速度会比从合理散布开始快很多。
% 粒子滤波去噪主循环
x_p = repmat(z(1), N, 1) + sqrt(R_sim) * randn(N, 1); % 初始化粒子
w = ones(N, 1) / N;
x_est = zeros(T, 1);
x_est(1) = z(1);
for k = 2:T
x_p = a * x_p + sqrt(Q_sim) * randn(N, 1); % 预测:AR模型演化
innov = z(k) - x_p; % 量测残差
lik = exp(-0.5 * innov.^2 / R_sim); % 高斯似然
w = w .* lik; % 权重更新
w = w / sum(w); % 权重归一化
Neff = 1 / sum(w.^2); % 有效粒子数
if Neff < N / 2 % 退化判断
[x_p, w] = residualResample(x_p, w); % 残差重采样
end
x_est(k) = w' * x_p; % 加权平均作为去噪输出
end
这段代码把粒子滤波的五个步骤都串起来了。预测阶段只用了 a*x_p + sqrt(Q_sim)*randn ,没有外部控制量;权重更新阶段用观测噪声方差 R_sim 构造高斯核,越接近真实观测的粒子权重越大;归一化之后计算 Neff,低于阈值才触发重采样;最后用加权平均得到当前时刻的状态估计。需要注意 x_est(k) 不是单个粒子的值,而是所有粒子位置的加权平均,这也是粒子滤波去噪和普通滑动窗口平均的本质区别。
3.2 权重更新公式与对数域避坑
权重更新这行的 exp(-0.5 * innov.^2 / R_sim) 看起来简单,实际使用时很容易踩坑。当 T 到几百步后,权重连乘很快下溢,MATLAB 会把它算成 NaN 或者 0。常见做法是每次把对数似然先减去最大值,再做指数,这样每个时刻的权重最大值都是 1,归一化后结果不变。
loglik = -0.5 * innov.^2 / R_sim;
lik = exp(loglik - max(loglik)); % 减去最大值,避免下溢
w = w .* lik;
w = w / sum(w);
如果粒子滤波.m 里某一步之后 x_est 突然变成 0,先检查这一步是不是权重全为 0。另一个常见问题是 innov.^2 / R_sim 写成矩阵除法,MATLAB 会解释成求解线性方程组而不是逐元素除法。一维时不会暴露,扩展到二维目标跟踪时就会出现维度错误,建议用 ./ 显式逐元素除。
3.3 重采样实现与有效粒子数阈值
Neff 是判断粒子退化最直接的指标。当所有粒子权重都一样时 Neff=N,当某个粒子权重接近 1 时 Neff 接近 1。我一般把阈值设为 N/2,低于这个值就触发重采样。阈值设太高会频繁重采样,粒子多样性下降;设太低则权重退化已经发生,估计开始漂移。
function [x_new, w_new] = residualResample(x, w)
N = numel(w);
w_new = zeros(N, 1);
x_new = zeros(N, 1);
n_rep = floor(w * N); % 每个粒子的确定性复制数
idx = 1;
for i = 1:N
if n_rep(i) > 0
x_new(idx:idx+n_rep(i)-1) = x(i);
w_new(idx:idx+n_rep(i)-1) = 1 / N;
idx = idx + n_rep(i);
end
w(i) = w(i) - n_rep(i) / N;
end
if idx <= N
c = cumsum(w);
c = c / c(end);
r = rand(N - idx + 1, 1);
for j = 1:numel(r)
sel = find(c >= r(j), 1, 'first');
x_new(idx) = x(sel);
w_new(idx) = 1 / N;
idx = idx + 1;
end
end
end
残差重采样的逻辑是先把高权重粒子按整数部分确定复制,再把剩余粒子按小数部分随机采样,最后每个新粒子的权重都重置为 1/N。这样比完全随机抽样的多项式重采样方差更小,也比纯系统重采样容易实现。在 MATLAB 脚本中,这个函数要放在主脚本末尾,使用前确认粒子数 N 和权重长度一致。
| 重采样方法 | 实现难度 | 引入方差 | 适用场景 |
|---|---|---|---|
| 多项式重采样 | 低 | 高 | 快速验证算法流程 |
| 残差重采样 | 中 | 中 | 通用去噪实验 |
| 系统重采样 | 中 | 低 | 跟踪精度要求高时 |
4. 粒子滤波目标跟踪实战:状态扩展与量测缺失处理
4.1 从一维去噪到二维目标跟踪的状态扩展
一维信号去噪只有一个状态变量 x_k,而目标跟踪至少要描述平面位置和速度,状态向量变成 [px, vx, py, vy]^T 。如果还是用单变量模型,滤波器会丢失目标的速度信息,导致位置估计总是滞后。常见的近匀速模型用恒定速度假设,转移矩阵 F 是块对角结构,位置由上一时刻位置加上速度乘采样间隔得到。
N = 1000;
T = 60;
dt = 1;
F = [1 dt 0 0;
0 1 0 0;
0 0 1 dt;
0 0 0 1];
Q = diag([0.2, 0.1, 0.2, 0.1]); % 过程噪声协方差
R = diag([0.4, 0.4]); % 观测噪声协方差
x_p = zeros(4, N);
x_p(1, :) = 10 + 0.5 * randn(1, N); % 初始 px
x_p(3, :) = 20 + 0.5 * randn(1, N); % 初始 py
w = ones(1, N) / N;
est = zeros(2, T);
这里的状态向量中,vx 和 vy 不像位置那样有直接量测值,它们是通过连续时刻的位置变化被隐式估计出来的。 dt 影响速度对位置预测的贡献, Q 里的 0.2 和 0.1 分别代表对位置和速度噪声的信任程度。使用粒子滤波 matlab 实现时, F 必须是常数矩阵或者能根据当前状态计算,否则粒子预测会发散。
4.2 跟踪场景中的似然函数与量测缺失
跟踪和去噪的权重更新区别在于量测残差变成了二维向量。权重不再是单个标量的高斯核,而是要同时考虑 x、y 两个方向的偏差。用 R \ innov 相当于对残差做马氏距离变换,让两个方向的噪声方差不同也能被正确加权。实际跟踪中量测丢失很常见,比如目标被遮挡时雷达没有回波,此时就不更新权重,只让粒子继续按运动模型扩散。
for k = 2:T
% 预测
x_p = F * x_p + sqrt(Q) * randn(4, N);
% 有量测才更新权重,NaN表示这一帧丢失
if ~isnan(z(1, k))
innov = z(:, k) - x_p([1 3], :);
lik = exp(-0.5 * sum(innov .* (R \ innov), 1));
w = w .* lik;
w = w / sum(w);
end
% 有效粒子数低于阈值才重采样
if 1 / sum(w.^2) < N / 2
[x_p, w] = residualResample(x_p, w);
end
est(:, k) = x_p([1 3], :) * w';
end
这段代码保留了去噪循环的基本骨架,但状态预测从一维标量变成了矩阵乘法。 z(1,k) 用 NaN 标记丢失帧,判断条件是 isnan 。当连续多帧没有量测时,粒子会靠运动模型继续外推,但协方差逐渐增大,等量测恢复后,权重更新会把发散拉回来。粒子跟踪的鲁棒性主要取决于这个阶段的重采样时机和 Q 矩阵的设置,Q 太小会导致粒子全部挤在预测路径上,丢失后无法重新捕获。
| 参数 | 作用 | 调参建议 |
|---|---|---|
| N | 每个时刻活跃的假设数量 | 1000 起步,实时性要求高时降到 500 |
| dt | 采样间隔 | 必须与观测帧率一致 |
| Q | 过程噪声 | 目标机动性强时调大,匀速场景调小 |
| R | 量测噪声 | 由传感器精度标定,不能盲目取 |
对于多目标跟踪,粒子滤波只负责单个目标的轨迹分布维护,真正的难点在于量测到目标的关联。工程里常见的做法是把粒子滤波与最近邻、JPDA 或者 DeepSORT 这类关联器组合,粒子滤波输出每个目标的后验期望,关联器决定哪一帧的观测属于哪个目标。单目标跟踪调好的 N、Q、R,在多目标场景下仍然可以作为每个目标各自独立维护时的默认参数。
4.3 跟踪结果后处理与验收
跟踪算法不能只看画出来的曲线。我一般会计算位置估计的 RMSE,并画出 Neff 随时间的变化,判断粒子是否在目标机动时突然退化。下面是常用的评估片段:
rmse = sqrt(mean(sum((est - x_true_pos).^2, 1)));
figure;
subplot(2,1,1);
plot(z(1,:), z(2,:), '.', est(1,:), est(2,:), '-');
subplot(2,1,2);
semilogy(Neff_history);
Neff_history 需要在循环里保存每一帧的 Neff。如果曲线频繁触底,说明粒子数或 Q 不匹配;如果全程接近 N,说明量测噪声 R 设得太大,粒子还没有被量测有效约束。
5. 重采样时机、粒子贫化与Neff诊断技巧
5.1 用Neff判断什么时候必须重采样
很多实现会在每帧都调用重采样,看起来粒子数始终饱满,实际上会带来粒子贫化。重采样的本质是用高权重粒子的副本替代低权重粒子,频繁执行会让所有粒子逐渐变成少数几个高权重样本的重复,最终失去多样性。常见做法是计算有效粒子数 Neff,低于阈值才触发。阈值不一定要固定为 N/2,也可以写成 N * 0.3 到 N * 0.7 之间的可配置参数。
Neff_history(k) = 1 / sum(w.^2);
if Neff_history(k) < N * 0.4
[x_p, w] = residualResample(x_p, w);
end
把阈值从 N/2 调整到 N*0.4 后,重采样次数通常会减少三分之一左右。判断标准不是重采样越少越好,而是看状态估计的 RMSE 是否下降。如果 RMSE 没有明显变化但计算时间减短,阈值就可以保持。注意 Neff 计算必须基于归一化后的权重,没有归一化的 w 直接平方求和会得到错误结果。
5.2 重采样后的粒子贫化与缓解措施
即使只在 Neff 低时重采样,粒子的多样性仍然会缓慢下降。重采样之后,大量粒子会落在完全相同的位置上,如果过程噪声很小,下一轮预测也拉不开差距。一个很实用的技巧是在重采样后对每个粒子施加一个很小的 jitter,相当于不改变均值但增加方差。
% 重采样后加一点正则噪声,缓解粒子贫化
jitter = sqrt(Q_sim) * 0.2;
x_p = x_p + jitter * randn(size(x_p));
这个 jitter 不能太大,否则会抹掉量测更新建立起来的后验峰。我一般取过程噪声标准差的十分之一到五分之一,再通过 Neff 曲线的回弹幅度来判断是否合适。如果回弹过快,说明 jitter 过大,重新引入的噪声超过了观测约束。
最后再提醒一个 MATLAB 细节: 粒子滤波.m 如果作为脚本运行,子函数不能放在同一个文件里,需要把 residualResample 放到单独文件,或者把主逻辑改造成函数并让重采样作为嵌套函数。调试时看到 Function definitions in a script must appear at the end of the file 的错误,就是这个原因。将 Neff < N/2 调成 Neff < N/3 ,同时记录重采样触发次数,是判断 N 是否合理的快捷方式。
955



被折叠的 条评论
为什么被折叠?



