基于MATLAB的PHD（概率假设密度）滤波器实现

一、PHD滤波器基础实现

1. 核心函数定义

function [phd, num_targets] = PHD_Filter(measurements, model, prev_phd)
    % 输入参数：
    % measurements: 当前时刻量测集合 [x1,y1; x2,y2;...]
    % model: 包含运动模型、观测模型、过程噪声等参数的结构体
    % prev_phd: 上一时刻的PHD分布（高斯混合模型形式）
    
    % 预测阶段
    predicted_phd = PredictPHD(prev_phd, model);
    
    % 更新阶段
    updated_phd = UpdatePHD(predicted_phd, measurements, model);
    
    % 稀疏化处理（剪枝与合并）
    [phd, num_targets] = PruneMerge(updated_phd, model);
end

function predicted_phd = PredictPHD(prev_phd, model)
    % 高斯分量外推（线性运动模型）
    predicted_phd = [];
    for i = 1:numel(prev_phd.weights)
        mu = prev_phd.means(i,:);
        Sigma = prev_phd.covariances(:,:,i);
        weight = prev_phd.weights(i);
        
        % 状态转移（匀速模型）
        F = [1 1; 0 1]; % 2D运动模型
        mu_pred = F * mu';
        Sigma_pred = F * Sigma * F' + model.process_noise;
        
        predicted_phd = [predicted_phd; struct('mean', mu_pred', ...
            'cov', Sigma_pred, 'weight', weight)];
    end
    
    % 新生目标生成
    num_new = poissrnd(model.spawn_rate);
    for i = 1:num_new
        new_mean = model.spawn_region(:,1) + randn(2,1)*model.spawn_std;
        new_cov = model.spawn_cov;
        predicted_phd = [predicted_phd; struct('mean', new_mean', ...
            'cov', new_cov, 'weight', model.spawn_weight)];
    end
end

function updated_phd = UpdatePHD(predicted_phd, measurements, model)
    updated_phd = [];
    for i = 1:numel(measurements)
        z = measurements(i,:);
        for j = 1:numel(predicted_phd)
            % 计算似然函数（高斯似然）
            H = [1 0; 0 1]; % 观测矩阵
            R = model.measurement_noise;
            S = H * predicted_phd(j).cov * H' + R;
            K = predicted_phd(j).cov * H' / S;
            likelihood = mvnpdf(z', H*predicted_phd(j).mean', S);
            
            % 更新权重
            updated_weight = predicted_phd(j).weight * likelihood * model.detection_prob;
            if updated_weight > 1e-6
                updated_mean = predicted_phd(j).mean' + K*(z' - H*predicted_phd(j).mean');
                updated_cov = (eye(2) - K*H) * predicted_phd(j).cov;
                updated_phd = [updated_phd; struct('mean', updated_mean, ...
                    'cov', updated_cov, 'weight', updated_weight)];
            end
        end
    end
    
    % 未检测目标保留
    for j = 1:numel(predicted_phd)
        survived_weight = predicted_phd(j).weight * (1 - model.detection_prob);
        if survived_weight > 1e-6
            updated_phd = [updated_phd; struct('mean', predicted_phd(j).mean', ...
                'cov', predicted_phd(j).cov, 'weight', survived_weight)];
        end
    end
end

function [phd, num_targets] = PruneMerge(phd, model)
    % 权重剪枝
    phd = phd(phd.weight > model.prune_threshold);
    
    % 高斯分量合并（基于马氏距离）
    merged = [];
    while ~isempty(phd)
        current = phd(1);
        distances = arrayfun(@(x) MahalanobisDist(x.mean, current.mean, x.cov, current.cov), phd);
        neighbors = find(distances < model.merge_threshold);
        
        % 合并权重与均值协方差
        total_weight = sum([phd(neighbors).weight]);
        merged_mean = sum(cat(1, phd(neighbors).mean) .* [phd(neighbors).weight], 1)/total_weight;
        merged_cov = sum(cat(3, phd(neighbors).cov) .* reshape([phd(neighbors).weight],1,1,numel(phd(neighbors))), 3);
        
        merged = [merged; struct('mean', merged_mean', 'cov', merged_cov, 'weight', total_weight)];
        phd(neighbors) = [];
    end
    
    % 限制最大分量数
    if size(merged,1) > model.max_components
        [~, idx] = sort([merged.weight], 'descend');
        merged = merged(idx(1:model.max_components));
    end
    
    phd = merged;
    num_targets = sum([phd.weight]);
end

function d = MahalanobisDist(mu1, mu2, cov1, cov2)
    % 计算马氏距离
    diff = mu1 - mu2;
    inv_cov = inv(cov1 + cov2);
    d = sqrt(diff' * inv_cov * diff);
end

二、参数配置与仿真

1. 模型参数定义

model = struct();
model.process_noise = diag([0.1, 0.1]); % 过程噪声协方差
model.measurement_noise = diag([1, 1]); % 观测噪声协方差
model.detection_prob = 0.95; % 检测概率
model.spawn_rate = 0.2; % 新生目标率
model.spawn_region = [0, 100; 0, 100]; % 新生区域
model.spawn_std = 2; % 新生目标标准差
model.prune_threshold = 0.01; % 剪枝阈值
model.merge_threshold = 3; % 合并阈值
model.max_components = 50; % 最大高斯分量数

2. 仿真场景生成

% 真实目标运动（匀速模型）
true_targets = [10, 20, 1, 0; 30, 40, -1, 0.5]; % [x, y, vx, vy]

% 生成带噪声的量测
measurements = [];
for t = 1:10
    % 目标运动
    for i = 1:size(true_targets,1)
        true_targets(i,:) = true_targets(i,:) + [1,0;0,1] * true_targets(i,3:4)';
    end
    
    % 生成量测（添加噪声和杂波）
    num_z = 20; % 量测数
    z = [randn(num_z,1)*10 + 50, randn(num_z,1)*10 + 50]; % 杂波区域
    real_z = true_targets(:,1:2) + mvnrnd(zeros(1,2), model.measurement_noise, size(true_targets,1));
    measurements = [measurements; real_z; z];
end

3. PHD滤波过程

% 初始化PHD
initial_phd = struct('mean', [50,50]', 'cov', diag([100,100]), 'weight', 1);

% 迭代处理
tracked_objects = [];
for t = 1:size(measurements,1)
    [phd, num_targets] = PHD_Filter(measurements(t,:), model, initial_phd);
    initial_phd = phd;
    
    % 提取目标状态
    tracked_objects = [tracked_objects; struct('time', t, ...
        'means', cat(1, phd.mean), 'covs', cat(3, phd.cov))];
end

三、结果可视化

1. 目标轨迹对比

figure;
hold on;
for i = 1:size(true_targets,1)
    plot(true_targets(i,1), true_targets(i,2), 'r*-', 'LineWidth', 2);
end
for i = 1:size(tracked_objects,1)
    means = tracked_objects(i).means;
    plot(means(:,1), means(:,2), 'bo--', 'LineWidth', 1.5);
end
xlabel('X坐标'); ylabel('Y坐标');
legend('真实轨迹', 'PHD估计轨迹');
title('PHD滤波跟踪结果');

2. 目标数量估计

figure;
plot(1:size(tracked_objects,1), [tracked_objects.num_targets], 'r-o');
xlabel('时间步'); ylabel('估计目标数');
title('目标数量估计曲线');
grid on;

参考代码 PHD滤波程序代码 www.youwenfan.com/contentcng/53170.html

四、常见问题解决

目标漏检
- 增加检测概率pd
- 调整新生目标区域spawn_region
计算效率低
- 启用GPU加速（需Parallel Computing Toolbox）
- 限制最大分量数max_components
轨迹漂移
- 采用交互多模型(IMM)
- 增加过程噪声协方差