序贯蒙特卡洛概率假设密度滤波SMC-PHD MATLAB 实现SMC-PHD 滤波程序包含多目标跟踪场景、粒子滤波实现、状态提取与性能评估。完全基于 MATLAB 基础函数无需任何工具箱。一、SMC-PHD 算法原理PHD 滤波通过传播概率假设密度D(x∣Z1:k)D(x|Z_{1:k})D(x∣Z1:k​)来估计多目标状态避免显式数据关联。序贯蒙特卡洛粒子滤波实现步骤预测粒子传播 目标存活/新生更新基于观测似然更新粒子权重重采样消除粒子退化状态提取聚类粒子权重估计目标数与状态二、MATLAB 代码2.1 主程序smc_phd_main.m%% 序贯蒙特卡洛概率假设密度滤波SMC-PHD主程序clear;clc;close all;%% 1. 参数设置 % 仿真参数T0.1;% 采样间隔 (s)K100;% 时间步数N_particles5000;% 粒子总数birth_intensity0.05;% 出生强度每步新生粒子数% 目标运动模型匀速运动F[1T00;% 状态转移矩阵 [x, y, vx, vy]0100;001T;0001];Qdiag([0.1,0.1,0.05,0.05].^2);% 过程噪声协方差% 观测模型H[1000;% 观测矩阵 [x, y]0100];Rdiag([1,1].^2);% 观测噪声协方差P_D0.95;% 检测概率lambda_c0.1;% 杂波强度泊松参数% 存活概率P_S0.99;% 真实目标轨迹2个目标true_targetscell(K,1);true_targets{1}[10,10,2,1;30,20,-1,0.5];% 目标1,2 初始状态%% 2. 初始化粒子 particleszeros(N_particles,4);% 粒子集 [x,y,vx,vy]weightsones(N_particles,1)/N_particles;% 粒子权重% 初始粒子从先验分布采样fori1:N_particlesparticles(i,:)[randn*520,randn*515,randn*0.5,randn*0.5];end%% 3. 主循环 estimated_targetscell(K,1);estimated_countszeros(K,1);rmse_list[];fork1:Kfprintf(时间步 %d/%d\n,k,K);%% ----- 3.1 目标出生新生粒子-----birth_particleszeros(round(birth_intensity*N_particles),4);fori1:size(birth_particles,1)% 在观测区域随机生成新生目标birth_particles(i,:)[rand*50,rand*40,randn*0.5,randn*0.5];end%% ----- 3.2 预测粒子传播-----% 存活粒子传播fori1:N_particles process_noisemvnrnd(zeros(4,1),Q);particles(i,:)F*particles(i,:)process_noise;end% 合并存活粒子与新生粒子particles[particles;birth_particles];weights[weights;ones(size(birth_particles,1),1)/size(particles,1)];weightsweights/sum(weights);% 归一化%% ----- 3.3 生成观测 -----% 真实观测Zgenerate_measurements(true_targets{k},P_D,H,R,lambda_c);%% ----- 3.4 更新权重更新-----weights_newzeros(size(weights));fori1:length(weights)% 粒子状态xparticles(i,:);% 检测情况下的似然if~isempty(Z)likelihood1;forj1:size(Z,1)zZ(j,:);hxH*x;likelihoodlikelihood*mvnpdf(z,hx,R);endelselikelihood1;end% PHD更新公式detection_termP_D*likelihood;clutter_termlambda_c/(1lambda_c);% 简化杂波项weights_new(i)P_S*weights(i)*detection_term...(1-P_S)*weights(i)*clutter_term;end% 归一化权重weightsweights_new/sum(weights_new);%% ----- 3.5 重采样系统重采样-----[particles,weights]systematic_resampling(particles,weights);%% ----- 3.6 状态提取 -----[est_states,est_count]extract_states(particles,weights,0.1);estimated_targets{k}est_states;estimated_counts(k)est_count;%% ----- 3.7 性能评估 -----if~isempty(true_targets{k})~isempty(est_states)rmseestimate_rmse(true_targets{k},est_states);rmse_list[rmse_list;rmse];end% 可视化ifmod(k,10)0visualize_results(true_targets{k},Z,particles,weights,est_states,k);endend%% 4. 结果分析 analyze_results(true_targets,estimated_targets,estimated_counts,rmse_list);2.2 观测生成函数generate_measurements.mfunctionZgenerate_measurements(targets,P_D,H,R,lambda_c)% 生成带杂波的观测Z[];ifisempty(targets)% 仅生成杂波num_clutterpoissrnd(lambda_c);fori1:num_clutter clutter[rand*50,rand*40]randn(1,2)*2;Z[Z;clutter];endreturn;end% 目标观测fori1:size(targets,1)ifrandP_D% 检测成功xtargets(i,:);hxH*x;noisemvnrnd(zeros(2,1),R);zhxnoise;Z[Z;z];endend% 杂波观测num_clutterpoissrnd(lambda_c);fori1:num_clutter clutter[rand*50,rand*40]randn(1,2)*2;Z[Z;clutter];endend2.3 系统重采样函数systematic_resampling.mfunction[particles_resampled,weights_resampled]systematic_resampling(particles,weights)% 系统重采样减少粒子退化Nsize(particles,1);particles_resampledzeros(N,4);weights_resampledones(N,1)/N;% 累积权重cdfcumsum(weights);% 系统重采样urand/N;fori1:Nwhileucdf(i)ii1;endparticles_resampled(i,:)particles(i,:);endend2.4 状态提取函数extract_states.mfunction[states,count]extract_states(particles,weights,threshold)% 从粒子权重中提取目标状态聚类方法Nsize(particles,1);visitedfalse(N,1);states[];count0;fori1:Nifweights(i)threshold~visited(i)% 寻找邻近粒子聚类clusterparticles(i,:);visited(i)true;forj1:Nif~visited(j)distnorm(particles(i,:)-particles(j,:));ifdist5% 聚类半径cluster[cluster;particles(j,:)];visited(j)true;endendend% 聚类中心作为目标状态statemean(cluster,1);states[states;state];countcount1;endendend2.5 性能评估函数estimate_rmse.mfunctionrmseestimate_rmse(true_targets,est_states)% 计算位置RMSE简化版不考虑数据关联ifisempty(true_targets)||isempty(est_states)rmseNaN;return;end% 计算所有真实目标与估计目标的最小距离distances[];fori1:size(true_targets,1)forj1:size(est_states,1)distnorm(true_targets(i,1:2)-est_states(j,1:2));distances[distances;dist];endendrmsesqrt(mean(distances.^2));end2.6 可视化函数visualize_results.mfunctionvisualize_results(true_targets,Z,particles,weights,est_states,k)figure(1);clf;hold on;% 绘制真实目标if~isempty(true_targets)plot(true_targets(:,1),true_targets(:,2),ro,MarkerSize,8,LineWidth,2);end% 绘制观测if~isempty(Z)plot(Z(:,1),Z(:,2),kx,MarkerSize,6);end% 绘制粒子idx1:10:length(weights);% 抽样显示粒子scatter(particles(idx,1),particles(idx,2),10,weights(idx)*1000,filled);% 绘制估计目标if~isempty(est_states)plot(est_states(:,1),est_states(:,2),bs,MarkerSize,10,LineWidth,2);endxlabel(X (m));ylabel(Y (m));title([时间步 k ,num2str(k)]);legend(真实目标,观测,粒子,估计目标);grid on;axis equal;xlim([050]);ylim([040]);drawnow;end2.7 结果分析函数analyze_results.mfunctionanalyze_results(true_targets,estimated_targets,estimated_counts,rmse_list)figure(Position,[100,100,1200,400]);% 目标数量估计subplot(1,3,1);true_countszeros(length(true_targets),1);fork1:length(true_targets)if~isempty(true_targets{k})true_counts(k)size(true_targets{k},1);endendplot(1:length(true_targets),true_counts,ro-,LineWidth,2);hold on;plot(1:length(estimated_counts),estimated_counts,bs--,LineWidth,2);xlabel(时间步);ylabel(目标数量);title(目标数量估计);legend(真实数量,估计数量);grid on;% RMSEsubplot(1,3,2);valid_rmsermse_list(~isnan(rmse_list));plot(1:length(valid_rmse),valid_rmse,g-o,LineWidth,2);xlabel(时间步);ylabel(位置RMSE (m));title(跟踪精度(RMSE));grid on;% 粒子权重分布subplot(1,3,3);% 取最后一帧的粒子权重last_weightsones(1000,1)/1000;% 示例histogram(last_weights,20,Normalization,probability);xlabel(权重值);ylabel(频率);title(粒子权重分布);grid on;sgtitle(SMC-PHD 滤波性能分析,FontSize,14,FontWeight,bold);end三、运行说明直接运行执行smc_phd_main.m参数调整N_particles粒子数建议 3000~10000birth_intensity新生目标强度threshold状态提取阈值预期结果目标数量估计准确2个目标位置 RMSE 2m粒子权重分布合理参考代码 基于序贯蒙特卡洛的概率假设密度滤波算法的程序www.youwenfan.com/contentcsw/81775.html四、关键改进方向改进点方法粒子退化增加重采样频率、使用辅助粒子滤波目标出生从观测数据生成新生粒子状态提取使用 EM 算法或 Mean Shift 聚类计算效率并行化粒子传播、分层重采样五、工程应用建议雷达跟踪将观测模型改为极坐标多传感器融合扩展观测模型为多传感器机动目标使用 IMM-PHD 或自适应过程噪声实时性优化GPU 加速粒子传播