MATLAB 轨迹式 MHT（Track-Oriented MHT）

MHT_Track/
├── main_mht.m            % 一键运行示例
├── mht_filter.m          % MHT 主循环
├── gate_and_score.m      % 门控 + 得分
├── update_hypotheses.m   % 假设更新
├── prune_hypotheses.m    % N-scan 剪枝
├── plot_tracks.m         % 轨迹可视化
└── example/
    ├── detect.mat        % 模拟检测结果
    └── truth.mat         % 真值（可选）

function [tracks] = mht_filter(detect, param)
% detect : T×cell 每 cell 为 n×dim 观测
% param  : 结构体 Nscan, gate_thr, max_hyp
% tracks : 结构体数组，含 .id .x .P .age
​
T  = length(detect);
hyp0 = struct('id',0,'x',[],'P',[],'age',0,'logL',0,'parent',-1);
hypotheses = {hyp0};                       % 初始假设
tracks     = [];
​
for t = 1:T
    zt = detect{t};                        % 当前观测
    hypotheses = predict_step(hypotheses, t);   % 卡尔曼预测
    hypotheses = update_step(hypotheses, zt);   % 门控+更新+分支
    hypotheses = prune_hypotheses(hypotheses, param.Nscan);
end
​
% 提取最优轨迹
[~,best] = max([hypotheses.logL]);
tracks = backtrack(hypotheses, best);
end

function [gate_mask, score] = gate_and_score(z, x_pred, P_pred, S)
% 输出：gate_mask 逻辑向量，score 对数似然
innov = z - x_pred;
d2    = innov' * (S \ innov);
gate_mask = d2 < 9.21;                   % χ2(0.01,2)=9.21
score     = -0.5*(d2 + log(det(S)));
end

function hypotheses = prune_hypotheses(hypotheses, Nscan)
% 保留最近 Nscan 帧分支
depth = max([hypotheses.age]) - Nscan;
hypotheses = hypotheses([hypotheses.age] > depth);
% 再按得分保留 Top-K
[~,idx] = sort([hypotheses.logL], 'descend');
K = min(100, numel(hypotheses));
hypotheses = hypotheses(idx(1:K));
end

clear; clc; addpath('.');
​
%% 1. 生成模拟检测（3 目标交叉+遮挡）
load('example/detect.mat');   % 变量 detect 为 T×cell
param.Nscan   = 3;             % N-scan 剪枝深度
param.gate_thr = 9.21;         % 马氏距离门限
param.max_hyp  = 100;          % 最大假设数
​
%% 2. MHT 跟踪
tracks = mht_filter(detect, param);
​
%% 3. 可视化
plot_tracks(detect, tracks);