序贯蒙特卡洛概率假设密度滤波（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) 来估计多目标状态，避免显式数据关联。序贯蒙特卡洛（粒子滤波）实现步骤：

1. 预测：粒子传播 + 目标存活/新生
2. 更新：基于观测似然更新粒子权重
3. 重采样：消除粒子退化
4. 状态提取：聚类粒子权重估计目标数与状态

---

二、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 = [1 T 0 0;           % 状态转移矩阵 [x, y, vx, vy]
     0 1 0 0;
     0 0 1 T;
     0 0 0 1];
Q = diag([0.1, 0.1, 0.05, 0.05].^2);  % 过程噪声协方差

% 观测模型
H = [1 0 0 0;           % 观测矩阵 [x, y]
     0 1 0 0];
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; % 粒子权重

% 初始粒子从先验分布采样
for i = 1:N_particles
    particles(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 = [];

for k = 1:K
    fprintf('时间步 %d/%d\n', k, K);
    
    %% ----- 3.1 目标出生（新生粒子）-----
    birth_particles = zeros(round(birth_intensity*N_particles), 4);
    for i = 1:size(birth_particles,1)
        % 在观测区域随机生成新生目标
        birth_particles(i,:) = [rand*50, rand*40, randn*0.5, randn*0.5];
    end
    
    %% ----- 3.2 预测（粒子传播）-----
    % 存活粒子传播
    for i = 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));
    
    for i = 1:length(weights)
        % 粒子状态
        x = particles(i,:)';
        
        % 检测情况下的似然
        if ~isempty(Z)
            likelihood = 1;
            for j = 1:size(Z,1)
                z = Z(j,:)';
                hx = H * x;
                likelihood = likelihood * mvnpdf(z, hx, R);
            end
        else
            likelihood = 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
    
    % 可视化
    if mod(k,10)==0
        visualize_results(true_targets{k}, Z, particles, weights, est_states, k);
    end
end

%% ========== 4. 结果分析 ==========
analyze_results(true_targets, estimated_targets, estimated_counts, rmse_list);

---

2.2 观测生成函数 generate_measurements.m

function Z = generate_measurements(targets, P_D, H, R, lambda_c)
% 生成带杂波的观测
Z = [];

if isempty(targets)
    % 仅生成杂波
    num_clutter = poissrnd(lambda_c);
    for i = 1:num_clutter
        clutter = [rand*50, rand*40] + randn(1,2)*2;
        Z = [Z; clutter];
    end
    return;
end

% 目标观测
for i = 1:size(targets,1)
    if rand < P_D  % 检测成功
        x = targets(i,:)';
        hx = H * x;
        noise = mvnrnd(zeros(2,1), R)';
        z = hx + noise;
        Z = [Z; z'];
    end
end

% 杂波观测
num_clutter = poissrnd(lambda_c);
for i = 1:num_clutter
    clutter = [rand*50, rand*40] + randn(1,2)*2;
    Z = [Z; clutter];
end
end

---

2.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;
for i = 1:N
    while u > cdf(i)
        i = i + 1;
    end
    particles_resampled(i,:) = particles(i,:);
end
end

---

2.4 状态提取函数 extract_states.m

function [states, count] = extract_states(particles, weights, threshold)
% 从粒子权重中提取目标状态（聚类方法）
N = size(particles,1);
visited = false(N,1);
states = [];
count = 0;

for i = 1:N
    if weights(i) > threshold && ~visited(i)
        % 寻找邻近粒子（聚类）
        cluster = particles(i,:);
        visited(i) = true;
        
        for j = 1:N
            if ~visited(j)
                dist = norm(particles(i,:) - particles(j,:));
                if dist < 5  % 聚类半径
                    cluster = [cluster; particles(j,:)];
                    visited(j) = true;
                end
            end
        end
        
        % 聚类中心作为目标状态
        state = mean(cluster, 1);
        states = [states; state];
        count = count + 1;
    end
end
end

---

2.5 性能评估函数 estimate_rmse.m

function rmse = estimate_rmse(true_targets, est_states)
% 计算位置RMSE（简化版，不考虑数据关联）
if isempty(true_targets) || isempty(est_states)
    rmse = NaN;
    return;
end

% 计算所有真实目标与估计目标的最小距离
distances = [];
for i = 1:size(true_targets,1)
    for j = 1:size(est_states,1)
        dist = norm(true_targets(i,1:2) - est_states(j,1:2));
        distances = [distances; dist];
    end
end

rmse = sqrt(mean(distances.^2));
end

---

2.6 可视化函数 visualize_results.m

function visualize_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);
end

xlabel('X (m)'); ylabel('Y (m)');
title(['时间步 k = ', num2str(k)]);
legend('真实目标', '观测', '粒子', '估计目标');
grid on; axis equal;
xlim([0 50]); ylim([0 40]);
drawnow;
end

---

2.7 结果分析函数 analyze_results.m

function analyze_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);
for k = 1:length(true_targets)
    if ~isempty(true_targets{k})
        true_counts(k) = size(true_targets{k},1);
    end
end
plot(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;

% RMSE
subplot(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

---

三、运行说明

1. 直接运行 ：执行 smc_phd_main.m
2. 参数调整 ：
   • N_particles：粒子数（建议 3000~10000）
   • birth_intensity：新生目标强度
   • threshold：状态提取阈值
3. 预期结果 ：
   • 目标数量估计准确（2个目标）
   • 位置 RMSE < 2m
   • 粒子权重分布合理

参考代码 基于序贯蒙特卡洛的概率假设密度滤波算法的程序 www.youwenfan.com/contentcsw/81775.html

四、关键改进方向

改进点	方法
粒子退化	增加重采样频率、使用辅助粒子滤波
目标出生	从观测数据生成新生粒子
状态提取	使用 EM 算法或 Mean Shift 聚类
计算效率	并行化粒子传播、分层重采样

---

五、工程应用建议

1. 雷达跟踪：将观测模型改为极坐标
2. 多传感器融合：扩展观测模型为多传感器
3. 机动目标：使用 IMM-PHD 或自适应过程噪声
4. 实时性优化：GPU 加速粒子传播