MHT代码阅读(1)
MHT代码阅读(1)
文章目录
- MHT代码阅读(1)
-
- 1. FormTrackFamily
-
- 1.1 整体思路
- 1.2 代码实现
- 1.3 附录代码
1. FormTrackFamily
1.1 整体思路
轨道形成和维护：表示维护所有轨道的中央轨道文件以及在这些轨道上执行的操作
-
接收新的观测值
-
用轨迹分数（LLR）确认和删除轨迹、用门控技术减少后期计算
-
轨迹分数LLR
-
利用轨迹分数来确认和删除是经典的SPRT方法的应用，SPRT中将LLR当作轨迹分数。如图6-1，LLR（或者分数L），在跟阈值T1、T2相比较。
-
当L大于T2：宣布追踪确认
-
当L在T1、T2之间：继续测试
-
当L小于T1：删除轨迹
-
-
门控
- 一种计算效率高的方法是首先使用粗略形式的门控，例如分箱。 使用分箱方法，测量空间被划分为一个单元格（或分箱）网格。然后，一条轨迹只与它的分箱和相邻分箱中的观测值进行比较。 可以根据轨道质量和可用计算资源自适应地选择箱大小
- 使用门内的所有观察更新现有轨迹，并形成外推轨迹（未使用任何当前观察更新）。此外，基本上所有观测都用于形成新轨迹的第一个点。因此，可能形成大量轨迹并且许多轨迹是不一致的，因为相同的观测被用于多个轨迹。
- 当轨道没有共同的观察时，轨道被定义为兼容。通过在每次扫描之间保持先前不兼容的结果，可以实现效率的显着提高。因此，实际上对于每个轨道都有一个与该轨道不兼容的轨道列表。这种不兼容性会传递给从父轨道产生的后代轨道。
- 在MHT 中，门控的目的只是为了从进一步处理中消除极不可能的替代方案。
- 在 MHT中，通常选择更大的门，并使用 MHT 轨道修剪逻辑 来消除 不太可能的潜在轨道分支。
-
1.2 代码实现
-
初始化卡尔曼滤波矩阵
-
形成一个新的轨迹
- 设置ct存放当前观测数据
- 进入循环
- 初始化轨迹分数LLR
- 初始化状态向量
- 构建stateTreeSet结构体，用于存放tree，每个tree中包含Node和parent，Node是个元胞数组，存放的第一列是状态向量，后四列是协方差矩阵。
- 初始化scoreTreeSet（树的分数集合）、idTreeSet（树的编号集合）、activeTreeSet（活跃的树的集合）、obsMembership{i}（观察成员之间的关系）、familyID（集群编号）、trackID（轨迹编号）
-
用新的观察或者虚拟观察来更新轨迹
-
初始化treeDel=[]（要删除的树）、treeConfirmed=[]（保留的树）
-
进入外循环
-
ct1存储已确认轨迹中较差的将要被修剪的轨迹的数量
ct2存储已确认轨迹中好的无需修剪的轨迹数量
-
进入内循环
- 确认轨迹是否是活跃的，不活动的树枝保存在tabuList，这些树枝将不参与繁殖
- 用虚拟观察更新轨迹
- 当某轨迹没有被确认时
- 与other_param.dummyNumberTH比较，是在main中就设置好的阈值，意思是当我们遇到一个点观测不清楚，我们最多可以再继续观测次数不能超过这个阈值，如果连续观察次数超过阈值都无法准确观察，那么就将这个树废除。
- 当观察的数量不为0时
- 如果轨迹没有确认，那么继续用一个新的观察更新它（这一步相当于重复上一步）
- 保存观察成员的关系以加速 updateICL 函数
- 如果轨迹确认了，但是轨迹不够好，那么ct1+1
- confscTH：确认轨道修剪（MOT）。 平均检测置信度分数低于此阈值的已确认轨迹将被忽略。
- dummyRatioTH：基于虚拟观察数量与总观察数量的比率确认轨迹修剪，大于这个大于这个比率将会被忽略
- 如果轨迹确认了，并且足够好，那么ct2+1
- 如果轨迹没有确认，那么继续用一个新的观察更新它（这一步相当于重复上一步）
-
退出内循环
-
-
最终判断树是被删除、确认还是修剪
- 轨迹删除
- 对于刚确认的轨迹,如果这棵树的所有track的置信度都小于阈值,这棵树死亡
- 轨迹确认
- 确认轨迹的条件：轨迹所在的树只有这一条轨迹、这条轨迹置信度大于阈值、连续TH次miss
- 轨迹修剪
- 根据其分数修剪轨道分支
- 轨迹删除
-
1.3 附录代码
function [obsTreeSetNew,stateTreeSetNew,scoreTreeSetNew,idTreeSetNew,...
activeTreeSetNew,obsTreeSetConfirmed,stateTreeSetConfirmed,...
scoreTreeSetConfirmed,activeTreeSetConfirmed,familyID,trackID,treeDel,...
treeConfirmed,obsMembership]=formTrackFamily(obsTreeSetPrev,...
stateTreeSetPrev,scoreTreeSetPrev,idTreeSetPrev,activeTreeSetPrev, ...
obsTreeSetConfirmed,stateTreeSetConfirmed,scoreTreeSetConfirmed,activeTreeSetConfirmed,...
selectedTrackIDs,cur_observation,kalman_param,other_param,familyID,trackID,cur_time,dt,Mea)
---------------------------------------------------------------------------------------------------------------------------
observationNo=length(cur_observation.x);
familyNo=length(obsTreeSetPrev);
fai=[1 dt 0 0; %dt采样周期
0 1 0 0;
0 0 1 dt;
0 0 0 1];
qx=kalman_param.qx;
qy=kalman_param.qy;
%qf=kalman_param.qf;
kalman_Q_adjusted=[qx*dt^3/3 qx*dt^2/2 0 0;
qx*dt^2/2 qx*dt 0 0;
0 0 qy*dt^3/3 qy*dt^2/2;
0 0 qy*dt^2/2 qy*dt]; %这是kalman的过程噪声矩阵
%sen=cur_observation.sen;
if observationNo ~= 0 % ~=是不等于
obsTreeSet(observationNo,1)=tree;
stateTreeSet(observationNo,1)=tree;
scoreTreeSet(observationNo,1)=tree;
idTreeSet(observationNo,1)=tree;
activeTreeSet(observationNo,1)=tree;
obsMembership=cell(length(cur_observation.x),1);
else
obsTreeSet=[];
stateTreeSet=[];
scoreTreeSet=[];
idTreeSet=[];
activeTreeSet=[];
obsMembership=[];
end
---------------------------------------------------------------------------------------------------------------------------
%%
% start new tracks
ct=1; %ct存放当前观测时的数据，添加断点，ctrl+D，可以查看存储的数据
for i=1:observationNo % observationNo 当前观测的总长度
% initialize track score
loglik=1*(1/other_param.const); %other_param是一个结构体，在main 文件中可以看到包含的各种参数，loglik是LLR
[stateEstimate,kalman_initV_adjusted]=initState(cur_observation.x(i),cur_observation.y(i),Mea.R_mea{
i});%初始化一个状态向量
obsTreeSet(ct)=tree([cur_observation.x(i) cur_observation.y(i) cur_time 1 0 0 1]);
%last four elements 上面一行的1 0 0 1:
%(1) the number of observation nodes
%(2) the number of dummy nodes
%(3) the number of total dummy nodes
%(4) a dummy node indicator 一个虚拟节点指示器
stateTreeSet(ct)=tree([stateEstimate kalman_initV_adjusted]); %stateTreeSet是个结构体，多少个量测就存放着多少个tree，
%每个tree中包含Node和parent，Node是个元胞数组，存放的第一列是状态向量，
%后四列是协方差矩阵
scoreTreeSet(ct)=tree([loglik cur_observation.sc(i)]);
idTreeSet(ct)=tree([familyID trackID]);
activeTreeSet(ct)=tree(1);
obsMembership{
i}=[obsMembership{
i}; [familyID 1 trackID]];
familyID=familyID+1;
trackID=trackID+uint64(1);
ct=ct+1; %存储当前数据后，循环进入下一轮
end
---------------------------------------------------------------------------------------------------------------------------
%%
% update tracks with a new observation or a dummy observation. 虚拟观察
treeDel=[]; %存储删除的树，留到nScanPruning中删除
treeConfirmed=[]; %存储确认的树
for i=1:familyNo
treeInd=findleaves(obsTreeSetPrev(i));
tabuList=zeros(1,length(treeInd));
ct1=0; %存储已确认轨迹中较差的将要被修剪的轨迹的数量
ct2=0; % 存储已确认轨迹中好的无需修剪的轨迹数量
for j=1:length(treeInd)
% check if a track is active 不活动的树枝保存在tabuList，这些树枝将不参与繁殖
if activeTreeSetPrev(i).get(treeInd(j)) == 0
tabuList(j)=treeInd(j);
continue;
end
% update with a dummy observation
previousObservation=obsTreeSetPrev(i).get(treeInd(j));
stateEstimate=stateTreeSetPrev(i).get(treeInd(j)); %为上面状态估计在这个地方变成了一个矩阵
statePredict_missOBS=fai*stateEstimate(:,1);
scoreSel=scoreTreeSetPrev(i).get(treeInd(j));
loglik=scoreSel(1);
confsc=scoreSel(2);
ID_tmp=idTreeSetPrev(i).get(treeInd(j)); %tmp表示暂时
familyID_tmp=ID_tmp(1);
trackID_tmp=ID_tmp(2);
obsNo=previousObservation(4);
dummyNo=previousObservation(5)+1;
totalDummyNo=previousObservation(6)+1;
% if the track is not confirmed yet
% ，dummyNumberTH是在main中就设置好的阈值，意思是当我们遇到一个点观测不清楚，我们最多可以再继续观测5次，如果连续5次都无法准确观察，那么就将这个树废除
if dummyNo < other_param.dummyNumberTH %N_miss,dummy>N_miss也可能轨迹是活动的
% add penalty 加罚
loglik=max(loglik + (1/other_param.const)*log(1-other_param.pDetection),1*(1/other_param.const)); %这是LLR原始公式
vPredict=fai*stateEstimate(:,2:5)*fai'+...
((other_param.dummyNumberTH-dummyNo)/other_param.dummyNumberTH)*kalman_Q_adjusted;
statePredict=statePredict_missOBS;
else
statePredict=statePredict_missOBS;
vPredict=stateEstimate(:,2:5);
end
obsTreeSetPrev(i)=obsTreeSetPrev(i).addnode(treeInd(j),[previousObservation(1),...
previousObservation(2),cur_time,obsNo,dummyNo,totalDummyNo,NaN]);
stateTreeSetPrev(i)=stateTreeSetPrev(i).addnode(treeInd(j),[statePredict vPredict]);
scoreTreeSetPrev(i)=scoreTreeSetPrev(i).addnode(treeInd(j),[loglik confsc]);
idTreeSetPrev(i)=idTreeSetPrev(i).addnode(treeInd(j),[familyID_tmp trackID_tmp]);
if sum(selectedTrackIDs == trackID_tmp) ~= 1
activeTreeSetPrev(i)=activeTreeSetPrev(i).addnode(treeInd(j), 0);
else
activeTreeSetPrev(i)=activeTreeSetPrev(i).addnode(treeInd(j), 1);
end
%2019/6/26 17:55
if observationNo ~= 0
% update with a new observation if a track is not confirmed yet
if dummyNo < other_param.dummyNumberTH
[obsTreeSetPrev(i),stateTreeSetPrev(i),scoreTreeSetPrev(i),idTreeSetPrev(i),...
activeTreeSetPrev(i),trackID,obsUsed]=updateNewObservation(obsTreeSetPrev(i),...
stateTreeSetPrev(i),scoreTreeSetPrev(i),idTreeSetPrev(i),...
activeTreeSetPrev(i),treeInd(j),cur_observation,kalman_param,other_param,...
familyID_tmp,trackID,kalman_Q_adjusted,fai,Mea.R_mea); %这里是对数据的更新
% save observation memberships for speeding up the updateICL function
tryInd=find(obsUsed(:,1));
for k=1:length(tryInd)
obsMembership{
tryInd(k)}=[obsMembership{
tryInd(k)}; [i obsUsed(tryInd(k),2:3)]]; % [familyID branchIndex trackID]
end
elseif confsc/obsNo <= other_param.confscTH || (totalDummyNo-dummyNo)/(obsNo+totalDummyNo-dummyNo) >= other_param.dummyRatioTH
ct1=ct1+1; %confscTH：确认轨道修剪（MOT）。 平均检测置信度分数低于此阈值的已确认轨迹将被忽略。
%.dummyRatioTH：基于虚拟观察数量与总观察数量的比率确认轨迹修剪，大于这个比率将会被忽略
else
ct2=ct2+1; % count good tracks that have been confirmed
end
end
end
% tree deletion
tabuList=tabuList(tabuList ~= 0);
if ct1 == length(treeInd) - length(tabuList)
% tree deletion when all tree branches are dead
treeDel=[treeDel; i]; %对于刚确认的轨迹,如果这棵树的所有track的置信度都小于阈值,这棵树死亡
% tree confirmation
elseif ct2 == length(treeInd) - length(tabuList) && ct2 == 1
%确认轨迹的条件：轨迹所在的树只有这一条轨迹、这条轨迹置信度大于阈值、连续TH次miss
treeConfirmed=[treeConfirmed; i]; %length(treeInd)=1进入
% branch pruning
else
% prune track branches based on its score
activeTreeSetPrev(i)=activateTrackBranch(scoreTreeSetPrev(i),obsTreeSetPrev(i),activeTreeSetPrev(i),other_param,cur_time);
end
end
---------------------------------------------------------------------------------------------------------------------------
%%
obsTreeSetConfirmed=[obsTreeSetPrev(treeConfirmed); obsTreeSetConfirmed];
stateTreeSetConfirmed=[stateTreeSetPrev(treeConfirmed); stateTreeSetConfirmed];
scoreTreeSetConfirmed=[scoreTreeSetPrev(treeConfirmed); scoreTreeSetConfirmed];
activeTreeSetConfirmed=[activeTreeSetPrev(treeConfirmed); activeTreeSetConfirmed];
obsTreeSetNew=[obsTreeSetPrev; obsTreeSet];
stateTreeSetNew=[stateTreeSetPrev; stateTreeSet];
scoreTreeSetNew=[scoreTreeSetPrev; scoreTreeSet];
idTreeSetNew=[idTreeSetPrev; idTreeSet];
activeTreeSetNew=[activeTreeSetPrev; activeTreeSet];
---------------------------------------------------------------------------------------------------------------------------
end
function [Xe,Pe]=initState(x,y,R)
H = [1 0 0 0;
0 0 1 0];
Xe = [x 0 y 0]';
Pe = H' * R * H;
numDimensions = size(H,1);
for i = 1:numDimensions
Pe(i*2, i*2) = 20;
end
end