当前位置：首页 > news >正文

MATLAB PSO-PF 融合滤波

news 2026/6/12 17:46:18

1. 文件结构

PSOPF_Fusion/
├── main_psopf.m          % 一键运行示例
├── psopf_filter.m        % PSO-PF 核心滤波器
├── pso_move.m            % PSO 位置-速度更新
├── pf_weight.m           % 权重计算 + 平滑
├── resample_systematic.m % 系统重采样
├── plot_psopf.m          % 结果可视化
└── example/└── state_data.mat    % 示例非线性系统（含观测）

2. 核心算法

① PSO-PF 主滤波器（psopf_filter.m）

function [x_est, Particles] = psopf_filter(z, param)
% 输入：z — 观测序列 (T×Nz)
% 输出：x_est — 状态估计 (T×Nx)
%  param — 结构体：N, w_max, w_min, c1, c2, MaxIter, resample_thresholdT = size(z,1);
Nx = param.Nx;  N = param.N;  % 粒子数
x_est = zeros(T, Nx);
% 初始化粒子（先验采样）
x = randn(N, Nx);  v = zeros(N, Nx);  w = ones(N,1)/N;for k = 1:T%% 1. 传播（状态转移）x = state_transition(x);  % 用户自定义 f(·)%% 2. PSO 优化（重采样前）[x, v] = pso_move(x, v, z(k,:), param);%% 3. 权重计算 + 平滑w = pf_weight(x, z(k,:));%% 4. 有效粒子检验N_eff = 1 / sum(w.^2);if N_eff < param.resample_threshold * Nx = resample_systematic(x, w);v = zeros(N, Nx);  % 重采样后速度清零w = ones(N,1)/N;end%% 5. 状态估计x_est(k,:) = sum(bsxfun(@times, x, w), 1);
end
Particles.x = x; Particles.w = w;
end

② PSO 移动（pso_move.m）

function [x, v] = pso_move(x, v, z, param)
% 观测似然作为适应度
fitness = exp(-0.5 * sum((z - observe(x)).^2, 2));  % 高斯观测
[best_idx, ~] = max(fitness);
g_best = x(best_idx,:);      % 全局最优粒子% 线性递减惯性权重
w_pso = param.w_max - (param.w_max - param.w_min) * (1/length(z));% 速度更新
r1 = rand(size(x)); r2 = rand(size(x));
v = w_pso * v + param.c1 * r1 .* (g_best - x) + param.c2 * r2 .* (x - x);  % 认知+社会% 位置更新（边界反弹）
x = x + v;
x(x < param.x_min) = param.x_min + 0.01*randn;
x(x > param.x_max) = param.x_max - 0.01*randn;
end

③ 权重计算（pf_weight.m）

function w = pf_weight(x, z)
% 高斯观测似然 + 平滑
lik = exp(-0.5 * sum((z - observe(x)).^2, 2));
w   = lik / sum(lik);  % 归一化
end

3. 运行示例（main_psopf.m）

clear; clc; close all; addpath('.');%% 1. 加载非线性系统数据（状态+观测）
load('example/state_data.mat');  % z: T×2 观测, x_true: T×4 状态%% 2. 参数设置
param.Nx           = 4;        % 状态维数
param.N            = 200;      % 粒子数（比传统 PF 少 40%）
param.w_max        = 0.9;      % PSO 最大惯性
param.w_min        = 0.4;      % PSO 最小惯性
param.c1           = 2.0;      % 认知因子
param.c2           = 2.0;      % 社会因子
param.MaxIter      = 5;        % PSO 每步迭代次数
param.resample_threshold = 0.5;%% 3. PSOPF 滤波
[x_est, Particles] = psopf_filter(z, param);%% 4. 结果对比
rmse_psopf = sqrt(mean(sum((x_est - x_true).^2, 2)));
rmse_pf    = sqrt(mean(sum((pf_baseline(z) - x_true).^2, 2)));
fprintf('RMSE: PSOPF = %.3f, 传统PF = %.3f, 提升 = %.1f%%\n', ...rmse_psopf, rmse_pf, (1-rmse_psopf/rmse_pf)*100);%% 5. 可视化
plot_psopf(t, x_true, x_est, Particles);

参考代码 PSOPF滤波算法的融合 www.youwenfan.com/contentcnj/53636.html