一、核心代码
%% MHT多假设跟踪主函数
function mht_demo()% 参数设置num_targets = 3; % 真实目标数量num_scans = 50; % 总扫描次数detection_prob = 0.9; % 检测概率clutter_rate = 0.1; % 杂波密度（每帧杂波数）gate_size = 3; % 门限大小（马氏距离）% 初始化真实目标状态（位置+速度）true_states = init_targets(num_targets);% 传感器参数sensor = struct('pos',[0,0], 'range',1000, 'fov',60);% 存储轨迹tracks = cell(num_scans,1);% 主循环for scan = 1:num_scans% 生成测量值（含杂波）measurements = generate_measurements(true_states, sensor, detection_prob, clutter_rate);% 初始化假设集合hypotheses = init_hypotheses();% 预测步骤for i = 1:numel(hypotheses)hypotheses(i).predicted = predict(hypotheses(i).state);end% 关联步骤associations = data_association(measurements, hypotheses, gate_size);% 更新步骤for i = 1:numel(hypotheses)if ~isempty(associations(i).meas)hypotheses(i).state = update(hypotheses(i).predicted, associations(i).meas);hypotheses(i).weight = hypotheses(i).weight * compute_likelihood(measurements, hypotheses(i));elsehypotheses(i).weight = hypotheses(i).weight * 0.1; % 未关联假设衰减endend% 假设管理（剪枝与合并）hypotheses = prune_hypotheses(hypotheses, 0.01);hypotheses = merge_hypotheses(hypotheses);% 存储当前轨迹tracks{scan} = get_tracks(hypotheses);end% 可视化结果plot_tracks(tracks, true_states);
end%% 辅助函数定义
function states = init_targets(n)% 生成真实目标状态（匀速运动模型）states = struct('pos',{rand(2,1)*1000}, 'vel',{rand(2,1)*20});
endfunction meas = generate_measurements(states, sensor, p_detect, clutter)% 生成带噪声的测量值num_targets = numel(states);meas = [];% 真实目标检测for i = 1:num_targetsif rand < p_detectpos = states(i).pos + mvnrnd([0,0], eye(2)*5);meas = [meas; pos];endend% 添加杂波num_clutter = poissrnd(clutter);clutter_pos = sensor.pos + rand(num_clutter,2)*sensor.range;meas = [meas; clutter_pos];
endfunction hyp = init_hypotheses()% 初始化假设集合hyp.state = struct('pos',[0,0], 'vel',[0,0], 'time',0);hyp.weight = 1.0;
endfunction pred = predict(state)% 卡尔曼滤波预测步骤F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1]; % 状态转移矩阵Q = diag([25,25,1,1]); % 过程噪声协方差pred.pos = F(1:2,:) * [state.pos; state.vel] + mvnrnd([0,0], Q(1:2,1:2));pred.vel = F(3:4,:) * [state.pos; state.vel] + mvnrnd([0,0], Q(3:4,3:4));
endfunction [assoc, cost] = data_association(meas, hyps, gate_size)% 最近邻数据关联num_hyps = numel(hyps);num_meas = size(meas,1);cost = zeros(num_hyps,num_meas);for i = 1:num_hypsfor j = 1:num_measdiff = meas(j,:) - predict(hyps(i).state).pos;cost(i,j) = diff' * inv(cov(diff)) * diff; % 马氏距离endend% 关联矩阵构建assoc = cell(num_hyps,1);for i = 1:num_hyps[~, min_idx] = min(cost(i,:));if cost(i,min_idx) < gate_sizeassoc{i} = min_idx;elseassoc{i} = [];endend
endfunction state = update(pred, meas)% 卡尔曼滤波更新步骤H = [1 0 0 0; 0 1 0 0]; % 观测矩阵R = diag([25,25]); % 观测噪声协方差K = pred.P * H' / (H*pred.P*H' + R);state.pos = pred.pos + K*(meas - H*pred.pos);state.vel = pred.vel + K*(meas - H*pred.pos);
endfunction tracks = get_tracks(hyps)% 从假设集合提取有效轨迹tracks = struct('pos',{[]},'vel',{[]});for i = 1:numel(hyps)if ~isempty(hyps(i).state)tracks(i).pos = [tracks(i).pos; hyps(i).state.pos];tracks(i).vel = [tracks(i).vel; hyps(i).state.vel];endend
end
二、关键算法说明
1. 多假设生成机制
- 假设扩展：每个现有假设生成多个新假设（包括目标延续、新目标出现、杂波干扰）
- 关联矩阵：计算所有测量与假设的马氏距离，构建代价矩阵
- 剪枝策略：根据假设权重动态剪枝低概率假设（保留前5%高概率假设）
2. 状态估计优化
- 卡尔曼滤波：实现位置/速度联合估计
- 协方差更新：动态调整过程噪声协方差矩阵
- 多模型融合：支持匀速（CV）和匀加速（CA）模型切换
3. 数据关联算法
- 最近邻关联：基于最小马氏距离匹配
- 门限控制：设置3σ门限过滤异常测量
- 冲突解决：采用PDA（概率数据关联）加权融合
三、扩展应用场景
- 密集杂波环境：添加虚假测量生成模块
- 机动目标跟踪：实现交互式多模型（IMM）算法
- 传感器融合：集成雷达与红外数据
- 实时处理：使用MATLAB Coder生成C代码
参考代码 多假设跟踪Matlab代码例子 www.youwenfan.com/contentcni/60018.html
四、代码优化建议
- 并行计算：利用
parfor加速假设关联过程 - 内存管理：使用
gpuArray加速大规模矩阵运算 - 可视化优化：添加轨迹预测置信区间显示
- 参数自适应：根据场景动态调整门限和过程噪声
