judge.asv

IMM滤波的 matlab程序

function [track,SaveTrack,tracknum,frameleavings,DeleteorNot,deletetracknum,ZZ]=Judge(track,SaveTrack,framedata,frameleavings,tracknum,num,spt,DeleteorNot,Predict_Gate,deletetracknum);
%本函数用于对第三帧以后的数据进行判断是否属于已知的某条航迹，是则进行航迹的调整，否则将相应的帧数据中放入暂存矩阵中，进行滑窗判断



for i=1:tracknum
    t=1;
    for j=1:num    
        distants=abs(track(i,5,spt-1)-framedata(j,2));
        if distants<=Predict_Gate  %判断是否找到符合航迹要求的量测点
            framedata(j,5)=1;
            ZZ(:,t,i)=[framedata(j,2),framedata(j,3),framedata(j,4)];
            DeleteorNot(spt,i)=0; %这里加一个标志DeleteorNot，对已经起始的航迹却没找到符合的点的判断
            t=t+1;
        end
        dis(j)=distants;
    end
    
    if t>1
       k=find(min(dis));
       tracktemp(i,:)=[i,framedata(j,2),framedata(j,3),framedata(j,4),0,0,0];
       track(i,:,spt)=tracktemp(i,:); 
    end
    
    if t==1
       DeleteorNot(spt,i)=DeleteorNot(spt-1,i)+1;
       %对某一条没找到点的航迹进行处理
       tracktemp(i,:)=[i,track(i,5,spt-1),track(i,6,spt-1),track(i,7,spt-1),track(i,5,spt-1),track(i,6,spt-1),track(i,7,spt-1)];       
       track(i,:,spt)=tracktemp(i,:); 
       ZZ(:,t,i)=[track(i,5,spt-1),track(i,6,spt-1),track(i,7,spt-1)];
    end
    
    if DeleteorNot(spt,i)==2
        [SaveTrack,track,deletetracknum]=DeleteTrack(track,SaveTrack,i,spt,tracknum,deletetracknum);   
        tracknum=tracknum-1;       
    end   
end

%需要添加处理维数的程序，因为储存的航迹维数是变化的
% track(:,:,spt)=tracktemp;
%或者可以设定最大航迹数    

tl=1;
for i=1:num
    if framedata(i,5)==0
        frameleavings(tl,:,spt)=framedata(i,:);
        %这有点问题
        tl=tl+1;
    end
end