多目标跟踪 联合概率数据关联滤波器（JPDA）Matlab代码 实时画出目标与杂波 计算每个目标RMSE误差

今天咱们来玩点实战的，用Matlab实现多目标跟踪的JPDA算法。这个联合概率数据关联滤波器听着玄乎，其实就是帮咱们解决"哪个测量值属于哪个目标"的头痛问题。先看效果再拆解——程序运行时会实时显示蓝色三角代表真实轨迹，红色圆点是滤波器估计位置，绿色叉叉是传感器捕获的杂波。

初始化环境得讲究

% 清空工作区避免变量污染
clear; clc; close all;
% 多目标参数
true_positions = [20 40; 60 80; 100 120]; % 三个目标的真实路径起点
num_targets = size(true_positions,1);
dt = 0.5; % 时间步长
% 运动模型（匀速）
F = [1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1]; 
H = [1 0 0 0; 0 0 1 0]; % 观测矩阵

这里用匀速模型，状态向量包含位置和速度。F矩阵里的dt控制着运动预测的精细程度——调太小会导致计算量暴增，太大又会让跟踪延迟明显。

生成动态杂波有门道

function measurements = generate_measurements(true_pos, time_step)
    % 真实目标生成带噪声的观测
    meas_noise = 3*randn(size(true_pos));
    valid_meas = true_pos * diag([time_step, time_step]) + meas_noise;
    
    % 随机杂波生成（数量服从泊松分布）
    clutter_num = poissrnd(2);
    clutter_meas = 150*rand(clutter_num,2);
    
    measurements = [valid_meas; clutter_meas]; 
end

泊松分布模拟杂波数量更符合实际场景。注意这里把真实位置随时间扩散，制造目标移动效果，乘time_step让目标沿对角线移动方便观察。

多目标跟踪 联合概率数据关联滤波器（JPDA）Matlab代码 实时画出目标与杂波 计算每个目标RMSE误差

关联概率计算是核心

function beta = jpda_association(estimates, measurements)
    num_targets = size(estimates,1);
    num_meas = size(measurements,1);
    cost_matrix = zeros(num_targets, num_meas);
    
    % 计算每个目标与量测的马氏距离
    for i = 1:num_targets
        for j = 1:num_meas
            residual = measurements(j,:) - H*estimates(i).x;
            cost_matrix(i,j) = residual / estimates(i).P * residual';
        end
    end
    
    % 这里简化处理——实际JPDA需要计算所有可能关联的概率
    [assign,~] = assignmentoptimal(cost_matrix); % 使用匈牙利算法
    beta = zeros(num_targets, num_meas+1);
    for i = 1:num_targets
        if assign(i) ~= 0
            beta(i,assign(i)) = 1; 
        else
            beta(i,end) = 1; % 未关联的概率
        end
    end
end

注意这里用了匈牙利算法进行最邻近关联，实际JPDA应该计算所有可能联合事件的概率。为了实时性我们做了简化，毕竟在杂波密度不高时效果也够用。

实时绘图与误差统计

figure('Position',[100 100 800 600]);
hold on;
grid on;
h_true = plot(0,0,'b^','MarkerSize',10); % 真实轨迹
h_est = plot(0,0,'ro','MarkerFaceColor','r'); % 估计位置
h_meas = plot(0,0,'gx'); % 量测点

rmse = zeros(num_targets,1);
for k = 1:50 % 时间步循环
    % 更新真实位置并生成量测
    true_positions = true_positions + [ones(num_targets,1)*5, ones(num_targets,1)*3];
    z = generate_measurements(true_positions, k);
    
    % JPDA滤波更新
    estimates = jpda_update(estimates, z);
    
    % 实时绘图
    set(h_true,'XData',true_positions(:,1),'YData',true_positions(:,2));
    set(h_est,'XData',[estimates.x_pred(1)],'YData',[estimates.x_pred(3)]); 
    set(h_meas,'XData',z(:,1),'YData',z(:,2));
    drawnow limitrate;
    
    % 累积误差
    for m = 1:num_targets
        rmse(m) = rmse(m) + norm(true_positions(m,:) - estimates(m).x_pred([1,3])').^2;
    end
end
rmse = sqrt(rmse/50); % 输出最终RMSE

这里用了图形句柄动态更新而不是重新绘图，运行效率提升十倍不止。drawnow limitrate是Matlab的流畅绘图秘诀，比单纯的pause(0.1)更节省资源。

跑起来之后会发现，当两个目标距离较近时会出现短暂的ID交换现象——这是数据关联算法的通病。想要改善的话可以尝试在代价矩阵中加入运动方向一致性判断，或者引入颜色等特征信息。

完整代码里还有协方差矩阵更新、新生目标处理等模块，限于篇幅这里没全贴出来。重点记住：好的多目标跟踪不是追求零误差，而是保持稳定的身份维持能力。毕竟实战中目标突然消失又出现是常态，滤波器得扛得住这种不确定性冲击。