MATLAB 实现，涵盖了 SMC-PHD 滤波器的核心算法、可视化工具和性能评估。
1. 核心算法实现
1.1 主程序：main_SMC_PHD.m
matlab
%% 主程序：SMC-PHD 多目标跟踪
clear; clc; close all;
%% 参数设置
params = struct();
params.T = 100; % 总时间步数
params.dt = 1; % 时间步长
params.area = [0 100 0 100]; % 监控区域 [xmin xmax ymin ymax]
% 目标动态模型参数
params.motion_model = 'CV'; % 匀速模型
params.F = [1 params.dt 0 0; % 状态转移矩阵
0 1 0 0;
0 0 1 params.dt;
0 0 0 1];
params.Q = diag([0.1, 0.05, 0.1, 0.05]); % 过程噪声协方差
% 观测模型参数
params.H = [1 0 0 0; % 观测矩阵
0 0 1 0];
params.R = diag([2, 2]); % 观测噪声协方差
% 滤波器参数
params.N_particles = 5000; % 粒子总数
params.p_survival = 0.99; % 存活概率
params.p_detection = 0.95; % 检测概率
params.birth_intensity = 0.1; % 新生目标强度
params.clutter_rate = 5; % 杂波率
params.resample_threshold = 0.5; % 重采样阈值
params.prune_threshold = 1e-5; % 剪枝阈值
params.merge_distance = 4; % 合并距离
% 新生目标模型
params.birth_model.type = 'gaussian';
params.birth_model.mean = [20, 2, 20, 2; 80, -2, 20, 2; 50, 1, 80, -1]'; % 多个新生区域
params.birth_model.cov = repmat(diag([10, 1, 10, 1]), [1, 1, 3]);
%% 生成仿真数据
fprintf('生成仿真数据...\n');
[true_trajectories, measurements] = generate_simulation_data(params);
%% 初始化SMC-PHD滤波器
fprintf('初始化SMC-PHD滤波器...\n');
[particles, weights] = initialize_filter(params);
%% 主循环
fprintf('开始滤波...\n');
estimated_targets = cell(params.T, 1);
estimated_numbers = zeros(params.T, 1);
performance_metrics = struct();
for k = 1:params.T
fprintf('时间步 %d/%d\n', k, params.T);
% 获取当前时刻观测
z_k = measurements{k};
% 预测步
[particles, weights] = phd_prediction(particles, weights, params, k);
% 更新步
[weights, likelihood_map] = phd_update(particles, weights, z_k, params);
% 归一化权重
total_weight = sum(weights);
if total_weight > 0
weights = weights / total_weight;
end
% 重采样
effective_ratio = 1 / sum(weights.^2) / params.N_particles;
if effective_ratio < params.resample_threshold
[particles, weights] = systematic_resample(particles, weights);
end
% 剪枝
[particles, weights] = prune_particles(particles, weights, params.prune_threshold);
% 估计目标状态
[estimated_targets{k}, estimated_numbers(k)] = extract_target_states(...
particles, weights, params);
% 记录性能
performance_metrics(k).time = k;
performance_metrics(k).n_true = length(true_trajectories);
performance_metrics(k).n_est = estimated_numbers(k);
performance_metrics(k).particles = length(weights);
end
%% 性能评估
fprintf('\n性能评估:\n');
evaluate_performance(true_trajectories, estimated_targets, params);
%% 可视化
visualize_results(true_trajectories, measurements, estimated_targets, ...
particles, weights, params, performance_metrics);1.2 初始化函数：initialize_filter.m
matlab
function [particles, weights] = initialize_filter(params)
%% 初始化SMC-PHD滤波器
% 均匀分布初始化粒子
N = params.N_particles;
% 位置均匀分布
x_min = params.area(1); x_max = params.area(2);
y_min = params.area(3); y_max = params.area(4);
particles = zeros(4, N);
particles(1, :) = x_min + (x_max - x_min) * rand(1, N); % x位置
particles(2, :) = -2 + 4 * rand(1, N); % x速度
particles(3, :) = y_min + (y_max - y_min) * rand(1, N); % y位置
particles(4, :) = -2 + 4 * rand(1, N); % y速度
% 均匀权重
weights = ones(1, N) / N;
% 添加新生目标粒子
[birth_particles, birth_weights] = generate_birth_particles(params, 0.1*N);
% 合并
particles = [particles, birth_particles];
weights = [weights, birth_weights];
% 归一化
weights = weights / sum(weights);
end1.3 预测步：phd_prediction.m
matlab
function [predicted_particles, predicted_weights] = phd_prediction(...
particles, weights, params, k)
%% PHD预测步
N = size(particles, 2);
% 存活粒子传播
survival_mask = rand(1, N) < params.p_survival;
survival_particles = particles(:, survival_mask);
survival_weights = weights(survival_mask) * params.p_survival;
% 运动模型传播
N_survival = size(survival_particles, 2);
predicted_survival = zeros(4, N_survival);
for i = 1:N_survival
% 状态转移
predicted_survival(:, i) = params.F * survival_particles(:, i);
% 添加过程噪声
noise = mvnrnd(zeros(4,1), params.Q)';
predicted_survival(:, i) = predicted_survival(:, i) + noise;
% 边界处理
predicted_survival(1, i) = max(params.area(1), ...
min(params.area(2), predicted_survival(1, i)));
predicted_survival(3, i) = max(params.area(3), ...
min(params.area(4), predicted_survival(3, i)));
end
% 生成新生目标
N_birth = round(params.birth_intensity * N);
[birth_particles, birth_weights] = generate_birth_particles(params, N_birth);
% 合并存活粒子和新生粒子
predicted_particles = [predicted_survival, birth_particles];
predicted_weights = [survival_weights, birth_weights];
end1.4 更新步：phd_update.m
matlab
function [updated_weights, likelihood_map] = phd_update(...
particles, weights, measurements, params)
%% PHD更新步
N = size(particles, 2);
M = size(measurements, 2);
% 初始化更新权重
updated_weights = zeros(1, N);
likelihood_map = zeros(N, M);
if M == 0
% 无观测情况
updated_weights = (1 - params.p_detection) * weights;
return;
end
% 计算杂波项
clutter_volume = (params.area(2)-params.area(1)) * (params.area(4)-params.area(3));
clutter_intensity = params.clutter_rate / clutter_volume;
% 预计算观测似然
for m = 1:M
for n = 1:N
% 预测观测
z_pred = params.H * particles(:, n);
% 计算似然
innovation = measurements(:, m) - z_pred;
likelihood_map(n, m) = mvnpdf(innovation', [0, 0], params.R);
end
end
% 计算每个观测的归一化项
normalization_terms = zeros(1, M);
for m = 1:M
term = clutter_intensity;
for n = 1:N
term = term + params.p_detection * likelihood_map(n, m) * weights(n);
end
normalization_terms(m) = term;
end
% 更新每个粒子的权重
for n = 1:N
% 漏检项
miss_detection = (1 - params.p_detection) * weights(n);
% 检测项
detection_sum = 0;
for m = 1:M
if normalization_terms(m) > 0
detection_sum = detection_sum + (params.p_detection * ...
likelihood_map(n, m) * weights(n)) / normalization_terms(m);
end
end
updated_weights(n) = miss_detection + detection_sum;
end
% 处理数值稳定性
updated_weights(updated_weights < 1e-100) = 1e-100;
end1.5 目标状态提取：extract_target_states.m
matlab
function [target_states, n_targets] = extract_target_states(...
particles, weights, params)
%% 从PHD中提取目标状态
% 估计目标数量
total_weight = sum(weights);
n_targets = round(total_weight);
if n_targets == 0
target_states = [];
return;
end
% 复制粒子用于聚类
particle_positions = particles(1:2, :);
particle_weights = weights;
% 使用均值漂移聚类
target_states = zeros(2, n_targets);
cluster_centers = [];
for t = 1:n_targets
if isempty(particle_positions) || sum(particle_weights) == 0
break;
end
% 找到权重最大的粒子
[~, max_idx] = max(particle_weights);
center = particle_positions(:, max_idx);
% 均值漂移迭代
bandwidth = params.merge_distance;
converged = false;
iterations = 0;
max_iterations = 20;
while ~converged && iterations < max_iterations
old_center = center;
% 计算核函数
distances = sqrt(sum((particle_positions - center).^2, 1));
kernel = exp(-0.5 * (distances / bandwidth).^2);
% 加权平均
weighted_sum = sum(kernel .* particle_weights .* particle_positions, 2);
weight_sum = sum(kernel .* particle_weights);
if weight_sum > 0
center = weighted_sum / weight_sum;
end
% 检查收敛
if norm(center - old_center) < 0.1
converged = true;
end
iterations = iterations + 1;
end
% 保存聚类中心
target_states(:, t) = center;
cluster_centers = [cluster_centers, center];
% 移除已聚类的粒子
distances_to_center = sqrt(sum((particle_positions - center).^2, 1));
keep_mask = distances_to_center > 2 * bandwidth;
particle_positions = particle_positions(:, keep_mask);
particle_weights = particle_weights(keep_mask);
if isempty(particle_positions)
break;
end
% 重新归一化权重
particle_weights = particle_weights / sum(particle_weights);
end
% 如果找到的目标少于估计数量，裁剪
target_states = target_states(:, 1:min(n_targets, size(target_states, 2)));
n_targets = size(target_states, 2);
end2. 辅助函数
2.1 数据生成：generate_simulation_data.m
matlab
function [true_trajectories, measurements] = generate_simulation_data(params)
%% 生成多目标跟踪仿真数据
T = params.T;
% 定义目标轨迹
n_targets = 3;
true_trajectories = cell(n_targets, 1);
% 目标1：从左侧进入，向右移动
x1 = zeros(4, T);
x1(:, 1) = [20; 2; 20; 0];
for t = 2:T
x1(:, t) = params.F * x1(:, t-1) + mvnrnd([0,0,0,0], params.Q)';
end
true_trajectories{1} = x1;
% 目标2：从顶部进入，向下移动
x2 = zeros(4, T);
x2(:, 1) = [50; 1; 80; -2];
for t = 2:T
x2(:, t) = params.F * x2(:, t-1) + mvnrnd([0,0,0,0], params.Q)';
end
true_trajectories{2} = x2;
% 目标3：在中间出现，随机移动
x3 = zeros(4, T);
x3(:, 1) = [80; -1; 30; 1];
for t = 2:T
x3(:, t) = params.F * x3(:, t-1) + mvnrnd([0,0,0,0], params.Q)';
end
true_trajectories{3} = x3;
% 生成测量数据
measurements = cell(T, 1);
for t = 1:T
z_t = [];
% 来自真实目标的测量
for i = 1:n_targets
if rand < params.p_detection
z_true = params.H * true_trajectories{i}(:, t);
z_noisy = z_true + mvnrnd([0,0], params.R)';
% 边界检查
if z_noisy(1) >= params.area(1) && z_noisy(1) <= params.area(2) && ...
z_noisy(2) >= params.area(3) && z_noisy(2) <= params.area(4)
z_t = [z_t, z_noisy];
end
end
end
% 杂波
n_clutter = poissrnd(params.clutter_rate);
for c = 1:n_clutter
clutter = [params.area(1) + (params.area(2)-params.area(1))*rand;
params.area(3) + (params.area(4)-params.area(3))*rand];
z_t = [z_t, clutter];
end
measurements{t} = z_t;
end
end2.2 重采样函数：systematic_resample.m
matlab
function [resampled_particles, resampled_weights] = systematic_resample(particles, weights)
%% 系统重采样
N = size(particles, 2);
% 计算累积分布
cdf = cumsum(weights);
% 生成均匀随机数
u = (rand + (0:N-1)) / N;
% 重采样索引
indices = zeros(1, N);
i = 1;
for j = 1:N
while cdf(i) < u(j)
i = i + 1;
end
indices(j) = i;
end
% 重采样
resampled_particles = particles(:, indices);
resampled_weights = ones(1, N) / N;
end2.3 剪枝函数：prune_particles.m
matlab
function [pruned_particles, pruned_weights] = prune_particles(particles, weights, threshold)
%% 剪枝：移除权重小的粒子
% 保留权重大于阈值的粒子
keep_mask = weights > threshold;
pruned_particles = particles(:, keep_mask);
pruned_weights = weights(keep_mask);
% 重新归一化
if sum(pruned_weights) > 0
pruned_weights = pruned_weights / sum(pruned_weights);
end
end2.4 新生目标生成：generate_birth_particles.m
matlab
function [birth_particles, birth_weights] = generate_birth_particles(params, N_birth)
%% 生成新生目标粒子
birth_type = params.birth_model.type;
switch lower(birth_type)
case 'gaussian'
% 高斯混合新生模型
n_components = size(params.birth_model.mean, 2);
particles_per_component = ceil(N_birth / n_components);
birth_particles = [];
for c = 1:n_components
mean_c = params.birth_model.mean(:, c);
cov_c = params.birth_model.cov(:,:,c);
% 从高斯分布采样
particles_c = mvnrnd(mean_c', cov_c, particles_per_component)';
birth_particles = [birth_particles, particles_c];
end
% 截断到指定数量
if size(birth_particles, 2) > N_birth
birth_particles = birth_particles(:, 1:N_birth);
end
case 'uniform'
% 均匀分布新生模型
birth_particles = zeros(4, N_birth);
birth_particles(1, :) = params.area(1) + (params.area(2)-params.area(1)) * rand(1, N_birth);
birth_particles(3, :) = params.area(3) + (params.area(4)-params.area(3)) * rand(1, N_birth);
birth_particles(2, :) = -2 + 4 * rand(1, N_birth);
birth_particles(4, :) = -2 + 4 * rand(1, N_birth);
otherwise
error('未知的新生目标类型');
end
% 分配权重
birth_weights = ones(1, size(birth_particles, 2)) * (params.birth_intensity / size(birth_particles, 2));
end3. 性能评估函数
3.1 评估性能：evaluate_performance.m
matlab
function evaluate_performance(true_trajectories, estimated_targets, params)
%% 评估SMC-PHD滤波器性能
T = params.T;
n_targets = length(true_trajectories);
% 计算OSPA距离
ospa_distances = zeros(1, T);
ospa_localization = zeros(1, T);
ospa_cardinality = zeros(1, T);
c = 20; % 截断距离
p = 1; % 阶数
for t = 1:T
% 真实目标位置
true_positions = zeros(2, n_targets);
for i = 1:n_targets
true_positions(:, i) = params.H * true_trajectories{i}(:, t);
end
% 估计目标位置
est_positions = estimated_targets{t};
% 计算OSPA
[ospa_distances(t), ospa_localization(t), ospa_cardinality(t)] = ...
compute_ospa(true_positions, est_positions, c, p);
end
% 计算平均性能
avg_ospa = mean(ospa_distances);
avg_localization = mean(ospa_localization);
avg_cardinality = mean(ospa_cardinality);
% 计算目标数量估计误差
true_numbers = ones(1, T) * n_targets;
est_numbers = cellfun(@(x) size(x,2), estimated_targets);
cardinality_rmse = sqrt(mean((est_numbers - true_numbers).^2));
% 显示结果
fprintf('\n=== 性能评估结果 ===\n');
fprintf('平均OSPA距离: %.3f\n', avg_ospa);
fprintf('平均定位误差: %.3f\n', avg_localization);
fprintf('平均基数误差: %.3f\n', avg_cardinality);
fprintf('目标数量RMSE: %.3f\n', cardinality_rmse);
fprintf('===================\n');
% 绘制性能曲线
figure('Position', [100, 100, 1200, 400]);
subplot(1, 3, 1);
plot(1:T, ospa_distances, 'b-', 'LineWidth', 1.5);
xlabel('时间步');
ylabel('OSPA距离');
title('OSPA距离随时间变化');
grid on;
subplot(1, 3, 2);
hold on;
plot(1:T, true_numbers, 'k-', 'LineWidth', 2, 'DisplayName', '真实数量');
plot(1:T, est_numbers, 'r--', 'LineWidth', 1.5, 'DisplayName', '估计数量');
xlabel('时间步');
ylabel('目标数量');
title('目标数量估计');
legend('Location', 'best');
grid on;
subplot(1, 3, 3);
hold on;
plot(1:T, ospa_localization, 'g-', 'LineWidth', 1.5, 'DisplayName', '定位误差');
plot(1:T, ospa_cardinality, 'm-', 'LineWidth', 1.5, 'DisplayName', '基数误差');
xlabel('时间步');
ylabel('误差');
title('误差分解');
legend('Location', 'best');
grid on;
end3.2 计算OSPA距离：compute_ospa.m
matlab
function [ospa, localization_error, cardinality_error] = compute_ospa(...
X, Y, c, p)
%% 计算OSPA距离
m = size(X, 2);
n = size(Y, 2);
if m == 0 && n == 0
ospa = 0;
localization_error = 0;
cardinality_error = 0;
return;
elseif m == 0 || n == 0
ospa = c;
localization_error = 0;
cardinality_error = c;
return;
end
% 计算距离矩阵
d = zeros(m, n);
for i = 1:m
for j = 1:n
dist = norm(X(:, i) - Y(:, j));
d(i, j) = min(dist, c);
end
end
% 使用匈牙利算法求解最优分配
if m <= n
assignment = munkres(d);
assigned = assignment > 0;
% 计算定位误差
loc_error = 0;
for i = 1:m
if assigned(i)
loc_error = loc_error + d(i, assignment(i))^p;
end
end
localization_error = (loc_error / n)^(1/p);
% 计算基数误差
cardinality_error = c^p * (n - m) / n;
cardinality_error = cardinality_error^(1/p);
else
% 转置后求解
assignment = munkres(d');
assigned = assignment > 0;
loc_error = 0;
for j = 1:n
if assigned(j)
loc_error = loc_error + d(assignment(j), j)^p;
end
end
localization_error = (loc_error / m)^(1/p);
cardinality_error = c^p * (m - n) / m;
cardinality_error = cardinality_error^(1/p);
end
% 总OSPA
ospa = (localization_error^p + cardinality_error^p)^(1/p);
end4. 可视化函数
4.1 可视化结果：visualize_results.m
matlab
function visualize_results(true_trajectories, measurements, estimated_targets, ...
particles, weights, params, performance_metrics)
%% 可视化SMC-PHD滤波器结果
T = params.T;
n_targets = length(true_trajectories);
% 创建图形窗口
figure('Position', [50, 50, 1400, 900]);
% 1. 真实轨迹和估计轨迹
subplot(2, 3, 1);
hold on;
% 绘制真实轨迹
colors = {'r', 'g', 'b', 'm', 'c'};
for i = 1:min(n_targets, length(colors))
trajectory = true_trajectories{i};
plot(trajectory(1, :), trajectory(3, :), ...
[colors{i} '-'], 'LineWidth', 2, 'DisplayName', sprintf('目标 %d', i));
end
% 绘制估计轨迹
est_trajectory_x = zeros(T, 0);
est_trajectory_y = zeros(T, 0);
for t = 1:T
est_targets = estimated_targets{t};
if ~isempty(est_targets)
for i = 1:size(est_targets, 2)
if t <= size(est_trajectory_x, 2)
est_trajectory_x(t, i) = est_targets(1, i);
est_trajectory_y(t, i) = est_targets(2, i);
else
est_trajectory_x(t, i) = est_targets(1, i);
est_trajectory_y(t, i) = est_targets(2, i);
end
end
end
end
for i = 1:size(est_trajectory_x, 2)
plot(est_trajectory_x(:, i), est_trajectory_y(:, i), ...
'k--', 'LineWidth', 1, 'DisplayName', sprintf('估计 %d', i));
end
xlabel('X 位置');
ylabel('Y 位置');
title('目标轨迹');
legend('Location', 'best');
grid on;
axis equal;
xlim(params.area(1:2));
ylim(params.area(3:4));
% 2. 粒子分布
subplot(2, 3, 2);
hold on;
% 绘制粒子
scatter(particles(1, :), particles(3, :), 5, weights, 'filled');
% 绘制估计目标
last_estimates = estimated_targets{end};
if ~isempty(last_estimates)
scatter(last_estimates(1, :), last_estimates(2, :), ...
100, 'r', 'x', 'LineWidth', 2);
end
xlabel('X 位置');
ylabel('Y 位置');
title('粒子分布');
colorbar;
grid on;
xlim(params.area(1:2));
ylim(params.area(3:4));
% 3. 观测数据
subplot(2, 3, 3);
hold on;
for t = 1:T
z_t = measurements{t};
if ~isempty(z_t)
scatter(z_t(1, :), z_t(2, :), 10, 'k', '.', 'MarkerAlpha', 0.3);
end
end
xlabel('X 位置');
ylabel('Y 位置');
title('观测数据（含杂波）');
grid on;
xlim(params.area(1:2));
ylim(params.area(3:4));
% 4. 目标数量估计
subplot(2, 3, 4);
hold on;
true_n = ones(1, T) * n_targets;
est_n = [performance_metrics.n_est];
plot(1:T, true_n, 'k-', 'LineWidth', 2, 'DisplayName', '真实数量');
plot(1:T, est_n, 'r--', 'LineWidth', 1.5, 'DisplayName', '估计数量');
xlabel('时间步');
ylabel('目标数量');
title('目标数量估计');
legend('Location', 'best');
grid on;
% 5. 粒子数量变化
subplot(2, 3, 5);
particle_counts = [performance_metrics.particles];
plot(1:T, particle_counts, 'b-', 'LineWidth', 1.5);
xlabel('时间步');
ylabel('粒子数量');
title('粒子数量变化');
grid on;
% 6. 权重分布直方图
subplot(2, 3, 6);
histogram(weights, 50, 'Normalization', 'probability');
xlabel('粒子权重');
ylabel('频率');
title('权重分布');
grid on;
% 添加总体标题
sgtitle('SMC-PHD 多目标跟踪结果', 'FontSize', 16, 'FontWeight', 'bold');
end4.2 实时动画：animate_tracking.m
matlab
function animate_tracking(true_trajectories, measurements, estimated_targets, params)
%% 生成跟踪动画
T = params.T;
n_targets = length(true_trajectories);
% 创建图形窗口
figure('Position', [100, 100, 800, 600]);
for t = 1:T
clf;
hold on;
% 绘制监控区域
rectangle('Position', [params.area(1), params.area(3), ...
params.area(2)-params.area(1), params.area(4)-params.area(3)], ...
'EdgeColor', 'k', 'LineWidth', 1);
% 绘制真实目标
for i = 1:n_targets
if t <= size(true_trajectories{i}, 2)
pos = true_trajectories{i}([1,3], t);
plot(pos(1), pos(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
% 绘制轨迹
traj_len = min(20, t);
start_idx = max(1, t-traj_len+1);
trajectory = true_trajectories{i}([1,3], start_idx:t);
plot(trajectory(1, :), trajectory(2, :), 'r-', 'LineWidth', 1);
end
end
% 绘制观测
z_t = measurements{t};
if ~isempty(z_t)
scatter(z_t(1, :), z_t(2, :), 30, 'k', 'x', 'LineWidth', 1.5);
end
% 绘制估计目标
est_targets = estimated_targets{t};
if ~isempty(est_targets)
scatter(est_targets(1, :), est_targets(2, :), 50, 'b', 's', 'filled');
end
% 设置图形属性
xlabel('X 位置');
ylabel('Y 位置');
title(sprintf('时间步: %d/%d', t, T));
grid on;
axis equal;
xlim(params.area(1:2));
ylim(params.area(3:4));
% 添加图例
legend('真实目标', '观测', '估计目标', 'Location', 'best');
% 暂停以显示动画
pause(0.1);
% 保存帧（可选）
if nargout > 0
frame(t) = getframe(gcf);
end
end
% 返回帧（可选）
if nargout > 0
varargout{1} = frame;
end
end5. 匈牙利算法实现（用于OSPA计算）
matlab
function [assignment, cost] = munkres(costMatrix)
%% 匈牙利算法实现
% 确保是方阵
[n, m] = size(costMatrix);
if n > m
costMatrix = [costMatrix; Inf(n, n-m)];
elseif m > n
costMatrix = [costMatrix, Inf(m-n, m)];
end
n = max(n, m);
costMatrix = costMatrix(1:n, 1:n);
% 步骤1：行归约
for i = 1:n
min_val = min(costMatrix(i, :));
costMatrix(i, :) = costMatrix(i, :) - min_val;
end
% 步骤2：列归约
for j = 1:n
min_val = min(costMatrix(:, j));
costMatrix(:, j) = costMatrix(:, j) - min_val;
end
% 初始化
assignment = zeros(1, n);
col_covered = zeros(1, n);
row_covered = zeros(1, n);
% 查找独立零
for i = 1:n
for j = 1:n
if costMatrix(i, j) == 0 && ~row_covered(i) && ~col_covered(j)
assignment(i) = j;
row_covered(i) = 1;
col_covered(j) = 1;
break;
end
end
end
% 重置覆盖
row_covered(:) = 0;
col_covered(:) = 0;
% 查找覆盖所有零的最小行数
marked_rows = find(assignment == 0);
if isempty(marked_rows)
% 已找到完整分配
assignment = assignment(1:n);
cost = sum(costMatrix(sub2ind(size(costMatrix), 1:n, assignment)));
return;
end
% 标记行
row_covered(marked_rows) = 1;
% 简化版本：贪心分配
[~, assignment] = min(costMatrix, [], 2);
assignment = assignment';
% 计算成本
valid_assign = assignment > 0 & assignment <= n;
cost = sum(costMatrix(sub2ind(size(costMatrix), ...
find(valid_assign), assignment(valid_assign))));
end参考代码 序贯蒙特卡洛算法 www.youwenfan.com/contentcsv/113008.html
6. 使用说明
6.1 快速开始
matlab
% 1. 运行主程序
main_SMC_PHD;
% 2. 或者分步运行
% 加载参数
params = struct();
params.T = 50;
params.N_particles = 3000;
% ... 设置其他参数
% 生成数据
[true_trajectories, measurements] = generate_simulation_data(params);
% 初始化滤波器
[particles, weights] = initialize_filter(params);
% 运行滤波
estimated_targets = cell(params.T, 1);
for k = 1:params.T
[particles, weights] = phd_prediction(particles, weights, params, k);
[weights, ~] = phd_update(particles, weights, measurements{k}, params);
[estimated_targets{k}, ~] = extract_target_states(particles, weights, params);
end
% 可视化
visualize_results(true_trajectories, measurements, estimated_targets, ...
particles, weights, params, []);6.2 参数调整指南
matlab
% 关键参数及其影响
params = struct();
% 粒子数量：增加可提高精度但降低速度
params.N_particles = 5000; % 推荐：1000-10000
% 存活概率：控制目标持续存在的可能性
params.p_survival = 0.99; % 推荐：0.95-0.99
% 检测概率：传感器检测目标的概率
params.p_detection = 0.95; % 推荐：0.8-0.99
% 新生目标强度：控制新目标出现的频率
params.birth_intensity = 0.1; % 推荐：0.05-0.3
% 杂波率：每个时间步的杂波数量
params.clutter_rate = 5; % 推荐：1-20
% 重采样阈值：控制重采样频率
params.resample_threshold = 0.5; % 推荐：0.3-0.77. 扩展功能
7.1 多种运动模型支持
matlab
function F = get_motion_model(model_type, dt)
%% 获取运动模型
switch lower(model_type)
case 'cv' % 匀速模型
F = [1 dt 0 0;
0 1 0 0;
0 0 1 dt;
0 0 0 1];
case 'ca' % 匀加速模型
F = [1 dt 0.5*dt^2 0 0 0;
0 1 dt 0 0 0;
0 0 1 0 0 0;
0 0 0 1 dt 0.5*dt^2;
0 0 0 0 1 dt;
0 0 0 0 0 1];
case 'ctrv' % 恒定转率和速度
F = @(omega) [1 sin(omega*dt)/omega 0 -(1-cos(omega*dt))/omega;
0 cos(omega*dt) 0 -sin(omega*dt);
0 (1-cos(omega*dt))/omega 1 sin(omega*dt)/omega;
0 sin(omega*dt) 0 cos(omega*dt)];
otherwise
error('未知的运动模型');
end
end7.2 非线性观测模型
matlab
function likelihood = nonlinear_likelihood(particle, measurement, params)
%% 非线性观测似然
% 从粒子状态计算预测观测
x = particle(1);
y = particle(3);
% 距离和方位
range = sqrt(x^2 + y^2);
bearing = atan2(y, x);
% 预测观测
z_pred = [range; bearing];
% 计算似然
innovation = measurement - z_pred;
% 处理方位角环绕
if innovation(2) > pi
innovation(2) = innovation(2) - 2*pi;
elseif innovation(2) < -pi
innovation(2) = innovation(2) + 2*pi;
end
% 高斯似然
likelihood = mvnpdf(innovation', [0, 0], params.R);
end8. 故障排除
常见问题及解决方案：
-
粒子退化
matlab% 降低重采样阈值 params.resample_threshold = 0.3; % 增加粒子数量 params.N_particles = 10000; -
目标数量估计不准确
matlab% 调整新生目标强度 params.birth_intensity = 0.2; % 调整存活概率 params.p_survival = 0.98; -
计算速度慢
matlab% 减少粒子数量 params.N_particles = 2000; % 使用更简单的运动模型 params.motion_model = 'CV';