序贯蒙特卡洛概率假设密度滤波（SMC-PHD） MATLAB 实现
序贯蒙特卡洛概率假设密度滤波（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. 参数设置 ==========% 仿真参数T=0.1;% 采样间隔 (s)K=100;% 时间步数N_particles=5000;% 粒子总数birth_intensity=0.05;% 出生强度（每步新生粒子数）% 目标运动模型（匀速运动）F=[1T00;% 状态转移矩阵 [x, y, vx, vy]0100;001T;0001];Q=diag([0.1,0.1,0.05,0.05].^2);% 过程噪声协方差% 观测模型H=[1000;% 观测矩阵 [x, y]0100];R=diag([1,1].^2);% 观测噪声协方差P_D=0.95;% 检测概率lambda_c=0.1;% 杂波强度（泊松参数）% 存活概率P_S=0.99;% 真实目标轨迹（2个目标）true_targets=cell(K,1);true_targets{1}=[10,10,2,1;30,20,-1,0.5];% 目标1,2 初始状态%% ========== 2. 初始化粒子 ==========particles=zeros(N_particles,4);% 粒子集 [x,y,vx,vy]weights=ones(N_particles,1)/N_particles;% 粒子权重% 初始粒子从先验分布采样fori=1:N_particlesparticles(i,:)=[randn*5+20,randn*5+15,randn*0.5,randn*0.5];end%% ========== 3. 主循环 ==========estimated_targets=cell(K,1);estimated_counts=zeros(K,1);rmse_list=[];fork=1:Kfprintf('时间步 %d/%d\n',k,K);%% ----- 3.1 目标出生（新生粒子）-----birth_particles=zeros(round(birth_intensity*N_particles),4);fori=1:size(birth_particles,1)% 在观测区域随机生成新生目标birth_particles(i,:)=[rand*50,rand*40,randn*0.5,randn*0.5];end%% ----- 3.2 预测（粒子传播）-----% 存活粒子传播fori=1:N_particles process_noise=mvnrnd(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)];weights=weights/sum(weights);% 归一化%% ----- 3.3 生成观测 -----% 真实观测Z=generate_measurements(true_targets{k},P_D,H,R,lambda_c);%% ----- 3.4 更新（权重更新）-----weights_new=zeros(size(weights));fori=1:length(weights)% 粒子状态x=particles(i,:)';% 检测情况下的似然if~isempty(Z)likelihood=1;forj=1:size(Z,1)z=Z(j,:)';hx=H*x;likelihood=likelihood*mvnpdf(z,hx,R);endelselikelihood=1;end% PHD更新公式detection_term=P_D*likelihood;clutter_term=lambda_c/(1+lambda_c);% 简化杂波项weights_new(i)=P_S*weights(i)*detection_term+...(1-P_S)*weights(i)*clutter_term;end% 归一化权重weights=weights_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)rmse=estimate_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.m
functionZ=generate_measurements(targets,P_D,H,R,lambda_c)% 生成带杂波的观测Z=[];ifisempty(targets)% 仅生成杂波num_clutter=poissrnd(lambda_c);fori=1:num_clutter clutter=[rand*50,rand*40]+randn(1,2)*2;Z=[Z;clutter];endreturn;end% 目标观测fori=1:size(targets,1)ifrand<P_D% 检测成功x=targets(i,:)';hx=H*x;noise=mvnrnd(zeros(2,1),R)';z=hx+noise;Z=[Z;z'];endend% 杂波观测num_clutter=poissrnd(lambda_c);fori=1:num_clutter clutter=[rand*50,rand*40]+randn(1,2)*2;Z=[Z;clutter];endend2.3 系统重采样函数systematic_resampling.m
function[particles_resampled,weights_resampled]=systematic_resampling(particles,weights)% 系统重采样（减少粒子退化）N=size(particles,1);particles_resampled=zeros(N,4);weights_resampled=ones(N,1)/N;% 累积权重cdf=cumsum(weights);% 系统重采样u=rand/N;fori=1:Nwhileu>cdf(i)i=i+1;endparticles_resampled(i,:)=particles(i,:);endend2.4 状态提取函数extract_states.m
function[states,count]=extract_states(particles,weights,threshold)% 从粒子权重中提取目标状态（聚类方法）N=size(particles,1);visited=false(N,1);states=[];count=0;fori=1:Nifweights(i)>threshold&&~visited(i)% 寻找邻近粒子（聚类）cluster=particles(i,:);visited(i)=true;forj=1:Nif~visited(j)dist=norm(particles(i,:)-particles(j,:));ifdist<5% 聚类半径cluster=[cluster;particles(j,:)];visited(j)=true;endendend% 聚类中心作为目标状态state=mean(cluster,1);states=[states;state];count=count+1;endendend2.5 性能评估函数estimate_rmse.m
functionrmse=estimate_rmse(true_targets,est_states)% 计算位置RMSE（简化版，不考虑数据关联）ifisempty(true_targets)||isempty(est_states)rmse=NaN;return;end% 计算所有真实目标与估计目标的最小距离distances=[];fori=1:size(true_targets,1)forj=1:size(est_states,1)dist=norm(true_targets(i,1:2)-est_states(j,1:2));distances=[distances;dist];endendrmse=sqrt(mean(distances.^2));end2.6 可视化函数visualize_results.m
functionvisualize_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% 绘制粒子idx=1: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.m
functionanalyze_results(true_targets,estimated_targets,estimated_counts,rmse_list)figure('Position',[100,100,1200,400]);% 目标数量估计subplot(1,3,1);true_counts=zeros(length(true_targets),1);fork=1: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_rmse=rmse_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_weights=ones(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~10000）birth_intensity：新生目标强度threshold：状态提取阈值
- 预期结果：
- 目标数量估计准确（2个目标）
- 位置 RMSE < 2m
- 粒子权重分布合理
参考代码 基于序贯蒙特卡洛的概率假设密度滤波算法的程序www.youwenfan.com/contentcsw/81775.html
四、关键改进方向
| 改进点 | 方法 |
|---|---|
| 粒子退化 | 增加重采样频率、使用辅助粒子滤波 |
| 目标出生 | 从观测数据生成新生粒子 |
| 状态提取 | 使用 EM 算法或 Mean Shift 聚类 |
| 计算效率 | 并行化粒子传播、分层重采样 |
五、工程应用建议
- 雷达跟踪：将观测模型改为极坐标
- 多传感器融合：扩展观测模型为多传感器
- 机动目标：使用 IMM-PHD 或自适应过程噪声
- 实时性优化：GPU 加速粒子传播