f_initiate_tracks.asv

多目标跟踪 (三维)-算法matlab编制

function [new_target_total,x_init,p_init,asso_mes]=f_initiate_tracks(M,dm,measurement,mes_total,vmin,vmax,r,t,yz1,yz2,yz3,yz4)
%一步延迟航迹起始
%M:用于航迹起始的扫描周期数
%measurement:所有量测(按列)
%mes_total:各个时刻的量测个数
%用于起始若干新航迹
x_init=[];
p_init=[];

count=linspace(0,0,M);%各个时刻的假设数
count(1)=mes_total(1);


for i=1:mes_total(1)
    hp(1,i).matrix=i;%hp(k,i):直至k时刻第i个假设的量测向量组合
end
for k=1:2:M-1
       mes_index1=sum(mes_total(1:k))+1;
       mes_index2=sum(mes_total(1:k+1));
       for i=1:count(k)%对k时刻的所有假设
          if k==1%初始时刻，利用vmax和vmin确定候选量测区域
             mes_now=measurement(:,i);
             p_now=eye(dm*3);%单位阵
             a_now=[];
             for l=1:dm
               a_now=[a_now;mes_now(l);0;0];
             end
             [confirm_mes,confirm_mes_index,confirm_mes_total,a1,p1,d]=f_confirm1(k,t,mes_now,a_now,p_now,mes_total(k+1),measurement(:,mes_index1:mes_index2),vmin,vmax,r,yz1);
             %[confirm_mes,confirm_mes_index,confirm_mes_total,a1,p1,d]=f_confirm1(k,t,dm,mes_now,mes_total(k+1),measurement(:,mes_index1:mes_index2),vmin,vmax,r);
           else
             [confirm_mes,confirm_mes_index,confirm_mes_total,a1,p1,d]=f_confirm2(k,t,hp(k,i).a,hp(k,i).p,mes_total(k+1),measurement(:,mes_index1:mes_index2),r,yz2);
          end
          
          if k==M-1
             [mincost,min_index]=min(d);
             b=confirm_mes_index(min_index);
             count(k+1)=count(k+1)+1;
             [hp(k+1,count(k+1)).distance,hp(k+1,count(k+1)).matrix,hp(k+1,count(k+1)).a,hp(k+1,count(k+1)).p]=f_decide(dm,hp(k,i).matrix,b,min_index,a,p);%决策，仅保留其一
             break;%退出循环
          end
          
          if confirm_mes_total>0%当一步确认区域有候选量测时
             mes_index3=sum(mes_total(1:k+1))+1;
             mes_index4=sum(mes_total(1:k+2));
             b=[];
             a=[];
             p=[];
             cost=[];
             for l=1:confirm_mes_total%对于k+1时刻的所有确认量测，确定k+2时刻相应的候选量测
                a0=a1(:,l);%%%%%%%%%%%%%%%%%%%%%%
                p0=p1(:,(l-1)*dm*3+1:l*dm*3);
                [b1,cost_temp,a_temp,p_temp]=f_extend(r,k,t,a0,p0,confirm_mes_index(l),confirm_mes(:,l),mes_total(k+2),measurement(:,mes_index3:mes_index4),yz4);  
               %b1:可能关联矩阵，两列(分别对应k+1时刻和k+2时刻的候选量测编号)
                b=[b;b1];
                a=[a a_temp];
                p=[p p_temp];
                cost=[cost cost_temp];
             end
             if isempty(b)==0
               [mincost,min_index]=min(cost);
               count(k+2)=count(k+2)+1;% 计数加1
               [hp(k+2,count(k+2)).matrix,hp(k+2,count(k+2)).a,hp(k+2,count(k+2)).p]=f_decide(dm,hp(k,i).matrix,b,min_index,a,p);%决策，仅保留其一
             end
          end
          
      end
end
%最终决策
decide_track_no=[];
decide_track_total=0;

asso_mes=[];%被关联上的量测
for j=1:count(M)       
    [x_rt,p_rt,d_rt]=f_get_xp(M,t,hp(M,j).matrix,hp(M,j).a,hp(M,j).p,mes_total,measurement,r);
    if d_rt<yz3
       asso_mes=[asso_mes;hp(M,j).matrix];
       decide_track_no=[decide_track_no j];
       decide_track_total=decide_track_total+1;
       x_init=[x_init x_rt(:,M)];
       p_init=[p_init p_rt(:,(M-1)*dm*3+1:M*dm*3)];
   end
end
new_target_total=decide_track_total;