mainpro_last1.m

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

%mainpro_last1
clear all;
jpda_parameter;%参数设置，读取一些仿真参数
k=1;
u_all=[];
u_input=[];
u_output=[]; 
input_mes=[];
z_save=[];
input_mes_total=linspace(0,0,M);%M=7,input_mes_total=[0 0 0 0 0 0 0]

while k<=30 %end_time,but在jpda_parameter.m中,end_time=99?
    [measurement,mes_total]=f_receive_mes(k);%从雷达信号处理模块，接收量测数据。
    if isempty(mes_total)==1  %按照f_receive_mes的定义,则这个函数始终为真,则永远不会执行以下的语句
        k=k+1;
        continue;
    end
    %measurement=f_coordi_trans(measurement,mes_total);%For practice but not for simulation
    mes_used=linspace(0,0,mes_total);%标志该量测是否被利用

    
    if k<=M
        input_mes=[input_mes measurement];%按列存放量测数据
        input_mes_total(k)=mes_total;%input_mes_total(k),第k个时刻的量测总数
    end
    if k==M
        %航迹起始
       [new_target_total,x_new,p_new,asso_mes]=f_initiate_tracks(M,dm,input_mes,input_mes_total,vmin,vmax,r,t,yz1,yz2,yz3,yz4);%航迹起始//航迹起始可能有问题，生成的目标数不对new_target_total=0;?
       %x_new=all_x(:,13);
      % p_new=eye(6);
       
       x=x_new; %x:在仿真中，存储所有目标的真实运动状态
       
       target_state=linspace(0,0,new_target_total)+1;%新目标1，老目标0，已删除2;
       all_target=new_target_total;%所有目标数，包括新目标，老目标（正存在的，和已经删除的)
       for i=1:all_target
          track_history(i).x=[];%用于记载目标整个航迹历史
       end
       miss_detect=linspace(0,0,new_target_total);%目标未被检测到的周期累计
       exist_target=1:all_target;
       exist_target_total=all_target;
       
       
       %记载未关联量测，从而完成下一次航迹起始
       [input_mes,input_mes_total]=f_compute_remain(M,input_mes,input_mes_total,new_target_total,asso_mes);
       
    end
    
    
   if k>M
     for i=1:all_target%预测下一时刻的位置
       if target_state(i)==0  %老目标
          [z(i).predict,s(i).predict]=f_imm_predict1(i,jpda(i).x,jpda(i).p,u_input(:,i));%i为目标号
       end 
       if target_state(i)==1%新起始的目标
          [current(i).x,current(i).p]=f_get_xandp(x_new,p_new,i-all_target+new_target_total,state_dm);
          [z(i).predict, s(i).predict]=f_initiate_predict(current(i).x,current(i).p);
       end
     
       if max(s(i).predict(:))>400
          target_state(i)=2;%（异常处理1，跟踪丢失)
       end
       
       
    end
     
    
    
    u=[];%存储目标跟踪门向量
    exist_target=[];
    exist_target_total=0;
    for i=1:all_target
       if target_state(i)<2 %激活的目标
          u_temp=f_rect_gate2(z(i).predict,s(i).predict,r,kg);%for simulation
          %u_temp=f_rect_gate(i,z(i).predict,s(i).predict,r,kg);%for practice but not for simulation
          
          u=[u u_temp];%存储目标跟踪门向量
          exist_target=[exist_target i];%当前激活的目标列表
          exist_target_total=exist_target_total+1;%当前激活的目标总数
          v(i)=abs((u_temp(2)-u_temp(1))*(u_temp(4)-u_temp(3))*(u_temp(6)-u_temp(5)));
       end
    end
    
    u_all=[u_all u];
    %cl_matrix:按行存放聚向量
    [cl_total,cl_element_total,cl_matrix]=f_produce_cluster2(exist_target,exist_target_total,u);%聚形成%for simulation
    %[cl_total,cl_element_total,cl_matrix]=f_produce_cluster(target_total,u);%聚形成%for practice but not for simulation
   
  
    
    confirm_meas = [];
    temp_confirm_meas = [];
    for c=1:cl_total%循环所有的聚 
        u_c=[];
        for i=1:cl_element_total(c)
           temp=find(cl_matrix(c,i)==exist_target);
           u_c=[u_c u(:,temp)];%生成该聚中目标的跟踪门向量
        end
        %for simulation
        [miss_detect_flag,mes_flag,confirm_matrix,temp_confirm_meas,confirm_meas_total(c)]=f_confirm_measure2(cl_element_total(c),measurement,mes_total,u_c);%该函数用于确定该聚中所有量测
        %for practice but not for simulation
        %[confirm_matrix,confirm_meas,confirm_meas_total(c)]=f_confirm_measure(cl_element_total(c),cl_matrix(:,c),measurement,mes_total,u);%该函数用于确定该聚中所有量测
        confirm_meas = [confirm_meas temp_confirm_meas];
        for i=1:mes_total
            if mes_flag(i)==1
                mes_used(i)=1;
            end
        end
        for j=1:cl_element_total(c)
            target_no=cl_matrix(c,j);
            miss_detect_flag(target_no)=miss_detect_flag(j);
            miss_detect(target_no)=miss_detect(target_no)+miss_detect_flag(target_no);
            if miss_detect(target_no)>thresh_value
               target_state(target_no)=2;%目标删除
               temp=find(target_no==exist_target);
               x=[x(:,1:temp-1) x(:,temp+1:exist_target_total)];%just for simulation
               exist_target=[exist_target(1:temp-1) exist_target(temp+1:exist_target_total)];
               exist_target_total=exist_target_total-1;
           end
        end
        
        %确认矩阵拆分,联合事件生成
        aa=linspace(0,0,confirm_meas_total(c));
        b=zeros(100,confirm_meas_total(c));
        event_no=0;%此处有误,有待修改2002-11-4
       
        [event_no,b]=f_recursion_try(1,confirm_meas_total(c),cl_element_total(c)+1,confirm_matrix,event_no,aa,b);
        b=b(1:event_no,:)-1;%第i个可行事件中量测j源于编号为b(i,j)的目标。
        prob=linspace(0,0,event_no)+1;%记载可行事件的概率
        %[used,termi_indicate,target_total]=f_terminate_tracks();%航迹终止(每个航迹是否终止标志)
        
        %关联概率形成%指定量测源于指定目标的概率---关联概率 
        for l=1:event_no
           %w=zeros(confirm_meas_total(c),cl_element_total(c));%初始请零
           %event(l).assoc_vector:与该聚中确认量测数同维，标志该事件中各个量测关联的目标号（在聚中的局部编码）。
           %event(l).deta目标关联指示：与该聚中目标个数同维
           %event(l).tao量测关联指示：与该聚中确认量测个数同维
           [event(l).assoc_vector,event(l).deta,event(l).tao,event(l).w,event(l).xjs]=f_produce_w(b(l,:),confirm_meas_total(c),cl_element_total(c));   
           for j=1:confirm_meas_total(c)
              if event(l).tao(j)==1
                mbh=cl_matrix(c,event(l).assoc_vector(j)); %目标的整体编号
                gm=temp_confirm_meas(:,j)-z(mbh).predict;%新息
                s_now=s(mbh).predict;
                n(j)=exp(-0.5*gm'*inv(s_now)*gm)/sqrt(2*pi*det(s_now));
              else
                n(j)=1; 
              end
           end
     
           for j=1:confirm_meas_total(c)
             prob(l)= prob(l)*n(j);
           end
     
           for t=1:cl_element_total(c)
             mbh=cl_matrix(c,t);
             if event(l).deta(t)==1
               prob(l)=prob(l)*pd;
             else
               prob(l)=prob(l)*(1-pd);
             end
           end
           for j=1:confirm_meas_total(c)
              prob(l)= prob(l)*(lmd^event(l).xjs);
           end
       end  %跳出对联合事件号l的循环  
       %计算关联概率
       sumprob=0;
       for l=1:event_no
          sumprob=sumprob+prob(l);
       end
      
       
       for l=1:event_no
           if sumprob>0          
              prob(l)=prob(l)/sumprob;% 归一化   
           else
              prob(l)=1/event_no;  %%%%%%%%%%%异常处理2（避免由于所有可行事件的概率都太小导致的sumprob为零现象）
           end
       end
       if prob(1)>0.9 & confirm_meas_total(c)>0
           for l=1:event_no
               prob(l)=1/event_no;%%%%%%%%%%%异常处理3(强制确认量测发挥错误)
           end
       end
   
       asso_pro0=linspace(0,0,all_target);%目标源于虚警的概率
       asso_pro=zeros(confirm_meas_total(c),all_target);
       
       for l=1:event_no 
          for t=1:cl_element_total(c)
            for j=1:confirm_meas_total(c)
              if event(l).w(j,t)==1
                asso_pro(j,t)=asso_pro(j,t)+prob(l);
              end
            end
          end
       end
     
       for t=1:cl_element_total(c) 
           mbh=cl_matrix(c,t);%总体编号
           for j=1:confirm_meas_total(c)
              asso_pro(j,mbh)=asso_pro(j,t);%量测j关联目标mbh（此处为全局编号）的概率
           end
           if max(asso_pro(:,mbh))>0.9%异常处理4,(由于前次滤波误差的影响，造成当前时刻杂波量测的似然函数大于目标真实回波的似然函数)
              asso_pro(:,mbh)=1/confirm_meas_total(c);
           end
       end
   end  
       for c=1:cl_total %调用immjpda程序
       for m=1:cl_element_total(c)%调用 immjpda
           target_no=cl_matrix(c,m);
         
           if target_state(target_no)==0%老目标
              if c==1
               [jpda(target_no).x,jpda(target_no).p,immjpda(target_no).x,immjpda(target_no).p,u_output(:,target_no)]=f_immjpda_new2(state_dm,target_no,target_state(target_no),asso_pro(m,:),confirm_meas(:, 1:confirm_meas_total(c)),confirm_meas_total(c),jpda(target_no).x,jpda(target_no).p,u_input(:,m),v(target_no),pd,pg,lmd);%m or target_no
           else
               [jpda(target_no).x,jpda(target_no).p,immjpda(target_no).x,immjpda(target_no).p,u_output(:,target_no)]=f_immjpda_new2(state_dm,target_no,target_state(target_no),asso_pro(m,:),confirm_meas(:, (1 + confirm_meas_total(c-1)):confirm_meas_total(c)),confirm_meas_total(c),jpda(target_no).x,jpda(target_no).p,u_input(:,m),v(target_no),pd,pg,lmd);%m or target_no
           end
           z_save = [z_save, immjpda(target_no).x];    
           u_output=[u_output u_output(:,target_no)];
              u_input=u_output; 
   
          elseif target_state(target_no)==1%新目标

              current(target_no).x=[current(target_no).x current(target_no).x current(target_no).x current(target_no).x current(target_no).x current(target_no).x];
              current(target_no).p=[current(target_no).p current(target_no).p current(target_no).p current(target_no).p current(target_no).p current(target_no).p];
              %tempx=current(target_no).x;
              %tempx=[tempx tempx tempx tempx tempx tempx];
              %tempp=current(target_no).p;
              %tempp=[tempp tempp tempp tempp tempp tempp];
             temp_output=linspace(0,0,6)';
              temp_input=(linspace(0,0,6)+1/6)';
             if c==1
               [jpda(target_no).x,jpda(target_no).p,immjpda(target_no).x,immjpda(target_no).p,temp_output]=f_immjpda_new2(state_dm,target_no,target_state(target_no),asso_pro(m,:),confirm_meas(:, 1:confirm_meas_total(c)),confirm_meas_total(c),current(target_no).x,current(target_no).p,temp_input,v(target_no),pd,pg,lmd);%m or target_no
           else
               [jpda(target_no).x,jpda(target_no).p,immjpda(target_no).x,immjpda(target_no).p,temp_output]=f_immjpda_new2(state_dm,target_no,target_state(target_no),asso_pro(m,:),confirm_meas(:, (1 + confirm_meas_total(c-1)):confirm_meas_total(c)),confirm_meas_total(c),current(target_no).x,current(target_no).p,temp_input,v(target_no),pd,pg,lmd);%m or target_no
             % x=immjpda(target_no).x
             % p=immjpda(target_no).p
         end
         z_save = [z_save, immjpda(target_no).x];    
         u_output=[u_output temp_output];
              u_input =u_output ;
              target_state(target_no)=0;%表示为老目标 
          end
  
           track_history(target_no).x=[track_history(target_no).x immjpda(target_no).x];%整个航迹历史
       end
       %调用immjpda程序结束
       
    end%for c,结束对整个聚的循环
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    un_association_mes=[];
    un_association_mes_total=0;
    for i=1:mes_total
        if mes_used(i)==0
           un_association_mes=[un_association_mes measurement(:,i)];
           un_association_mes_total=un_association_mes_total+1;
        end
    end
    input_mes=[input_mes un_association_mes];
    input_mes_total=[input_mes_total un_association_mes_total];
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %track initiation
    [new_target_total,x_new,p_new,asso_mes]=f_initiate_tracks(M,dm,input_mes,input_mes_total,vmin,vmax,r,t,yz1,yz2,yz3,yz4);%航迹起始,未完成
    %p_new=eye(6)
    %记载未关联量测，从而完成下一次航迹起始
    [input_mes,input_mes_total]=f_compute_remain(M,input_mes,input_mes_total,new_target_total,asso_mes);
    
    if new_target_total>0
      temp=linspace(0,0,new_target_total)+1;
      target_state=[target_state temp];
      miss_detect=[miss_detect linspace(0,0,new_target_total)];
      
    end
     
    for i=1:new_target_total
        track_history(all_target+i).x=[];%用于记载目标整个航迹历史
        exist_target=[exist_target all_target+i];%更新当前激活的目标列表
    end   
    all_target=all_target+new_target_total;%包括原有目标（已删除+未删除），新目标
    exist_target_total=exist_target_total+new_target_total;%当前激活的目标总数 
    %end track initiation
  
   end% if k>M
   k=k+1  
   save 'z_save.mat' z_save;
end %end while