test.m

IMM滤波的 matlab程序

clear
clc

%第一部分，产生航迹


%第二部分，两点起始
Start_Gate=400;                      %起始门限
Predict_Gate=300;                     %预测门限
MaxFlame=9;                        %总帧数
MaxTrack=5;                        %最大航迹数
MaxLeavings=3;                      %最大杂波数
dataassociation=5;                  %关联起始时间
deletetracknum=0;                   %删除的航迹，用于作图
T=1;                                %采样间隔
time=60;                            
ZZ=0;
ep=1;

%读第一帧数据
spt=1;
[framedata1,num1,ep]=ReadFrameData(spt,ep);
framedata1

%读第二帧数据
spt=2;
[framedata2,num2,ep]=ReadFrameData(spt,ep);
framedata2

track(MaxTrack+1,7,MaxFlame)=0;
SaveTrack=track;
frameleavings(MaxLeavings,5,time)=0;
%两点航迹起始 函数为Track_Begin
[track,tracknum]=Track_Begin(track,framedata1,framedata2,num1,num2,Start_Gate);
% track
% framedata
% tracknum
% track1
% track2
% framedata1
% framedata2

spt=3;
DeleteorNot(MaxFlame,MaxTrack)=0;

 C0kcv=[1,0,0];
 P_jpda_cvk_1(:,:,1)=[1,0,0;
                      0,1,0;
                      0,0,1]*10000;
               
 Rmeasure=[65^2,0,0;
            0,0.001^2,0;
            0,0,0.001^2];
        
for i=1:tracknum
    X_jpda_cvk_1r(:,1,i)=[track(i,2,1);(track(i,2,2)-track(i,2,1))/T;eps];
    X_jpda_cvk_1theta(:,1,i)=[track(i,3,1);(track(i,3,2)-track(i,3,1))/T;eps];
    X_jpda_cvk_1q(:,1,i)=[track(i,4,1);(track(i,4,2)-track(i,4,1))/T;eps];
    P_jpda_cvk_1r(:,:,1,i)=P_jpda_cvk_1(:,:,1);
    P_jpda_cvk_1theta(:,:,1,i)=P_jpda_cvk_1(:,:,1);
    P_jpda_cvk_1q(:,:,1,i)=P_jpda_cvk_1(:,:,1);    

    MUr1(:,1,i)=[1/3;1/3;1/3];
    MUtheta1(:,1,i)=[1/3;1/3;1/3];
    MUq1(:,1,i)=[1/3;1/3;1/3];
end


 for spt=spt-1:dataassociation-1;  
ep
     [framedata,num,ep]=ReadFrameData(spt,ep);         
     for k=1:num
         tracktemp(k,:,spt)=[k,framedata(k,2),framedata(k,3),framedata(k,4)];
     end

     for j=1:tracknum %滤波
        Zcv_1r(:,spt,j)=[tracktemp(j,2,spt)]';
        Zcv_1theta(:,spt,j)=[tracktemp(j,3,spt)]';
        Zcv_1q(:,spt,j)=[tracktemp(j,4,spt)]';
        [X_jpda_cvk_1r(:,spt,j),P_jpda_cvk_1r(:,:,spt,j),P_jpda_cvkk1_1r(:,:,spt,j),S1r(:,spt,j),K1r(:,spt,j),Xkk11r(:,spt,j),MUr1(:,spt,j)]=fimm(spt,Rmeasure(1,1),X_jpda_cvk_1r(:,spt-1,j),P_jpda_cvk_1r(:,:,spt-1,j),MUr1(:,spt-1,j),T,Zcv_1r(:,spt,j),1);
        [X_jpda_cvk_1theta(:,spt,j),P_jpda_cvk_1theta(:,:,spt,j),P_jpda_cvkk1_1theta(:,:,spt,j),S1theta(:,spt,j),Ktheta1(:,spt,j),Xkk11theta(:,spt,j),MUtheta1(:,spt,j)]=fimm(spt,Rmeasure(2,2),X_jpda_cvk_1theta(:,spt-1,j),P_jpda_cvk_1theta(:,:,spt-1,j),MUtheta1(:,spt-1,j),T,Zcv_1theta(:,spt,j),2);
        [X_jpda_cvk_1q(:,spt,j),P_jpda_cvk_1q(:,:,spt,j),P_jpda_cvkk1_1q(:,:,spt,j),S1q(:,spt,j),K1q(:,spt,j),Xkk11q(:,spt,j),MUq1(:,spt,j)]=fimm(spt,Rmeasure(3,3),X_jpda_cvk_1q(:,spt-1,j),P_jpda_cvk_1q(:,:,spt-1,j),MUq1(:,spt-1,j),T,Zcv_1q(:,spt,j),2);

        X_jpda_cvk_1(:,spt,j)=[X_jpda_cvk_1r(1,spt,j);X_jpda_cvk_1theta(1,spt,j);X_jpda_cvk_1q(1,spt,j)];
        X_jpda_cvkk1_1(:,spt,j)=[Xkk11r(1,spt,j);Xkk11theta(1,spt,j);Xkk11q(1,spt,j)];  
        S_jpda_1(:,:,spt,j)=diag([S1r(:,spt,j),S1theta(:,spt,j),S1q(:,spt,j)]);  
        K_jpda_1(:,:,spt,j)=[K1r(:,spt,j),Ktheta1(:,spt,j),K1q(:,spt,j)];
        
        track(j,2,spt)=X_jpda_cvk_1r(1,spt,j);
        track(j,3,spt)=X_jpda_cvk_1theta(1,spt,j);
        track(j,4,spt)=X_jpda_cvk_1q(1,spt,j);
        track(j,5,spt)=Xkk11r(1,spt,j);
        track(j,6,spt)=Xkk11theta(1,spt,j);
        track(j,7,spt)=Xkk11q(1,spt,j);
     end
 end
 
tracknum
track

sumtime=0;
    frameR(:,:) = zeros(3,4);       % r 暂存的数据
    frameq(:,:) = zeros(3,4);      %  
    frametheta(:,:)= zeros(3,4);  

for spt=dataassociation:time
    [framedata,num,ep]=ReadFrameData(spt,ep);
    [track,SaveTrack,tracknum,frameleavings,DeleteorNot,deletetracknum,ZZ_jpda,P_jpda_cvk_1r,MUr1,P_jpda_cvk_1theta,MUtheta1,P_jpda_cvk_1q,MUq1]=Judge(track,SaveTrack,framedata,frameleavings,tracknum,num,spt,DeleteorNot,Predict_Gate,deletetracknum,P_jpda_cvk_1r,MUr1,P_jpda_cvk_1theta,MUtheta1,P_jpda_cvk_1q,MUq1);
%  ZZ_jpda
%  ZZ_jpda(:,:,1)
    frameR(:,:,dataassociation-1) = eye(3,4);       % r 暂存的数据
    frameq(:,:,dataassociation-1) = eye(3,4);      %  
    frametheta(:,:,dataassociation-1)=eye(3,4);  
    for  j=1:tracknum %滤波
        Zcv_1r(:,spt,j)=[track(j,2,spt)]';
        Zcv_1theta(:,spt,j)=[track(j,3,spt)]';
        Zcv_1q(:,spt,j)=[track(j,4,spt)]';
        [X_jpda_cvk_1r(:,spt,j),P_jpda_cvk_1r(:,:,spt,j),P_jpda_cvkk1_1r(:,:,spt,j),S1r(:,spt,j),K1r(:,spt,j),Xkk11r(:,spt,j),MUr1(:,spt,j)]=fimm(spt,Rmeasure(1,1),X_jpda_cvk_1r(:,spt-1,j),P_jpda_cvk_1r(:,:,spt-1,j),MUr1(:,spt-1,j),T,Zcv_1r(:,spt,j),1);
        [X_jpda_cvk_1theta(:,spt,j),P_jpda_cvk_1theta(:,:,spt,j),P_jpda_cvkk1_1theta(:,:,spt,j),S1theta(:,spt,j),Ktheta1(:,spt,j),Xkk11theta(:,spt,j),MUtheta1(:,spt,j)]=fimm(spt,Rmeasure(2,2),X_jpda_cvk_1theta(:,spt-1,j),P_jpda_cvk_1theta(:,:,spt-1,j),MUtheta1(:,spt-1,j),T,Zcv_1theta(:,spt,j),2);
        [X_jpda_cvk_1q(:,spt,j),P_jpda_cvk_1q(:,:,spt,j),P_jpda_cvkk1_1q(:,:,spt,j),S1q(:,spt,j),K1q(:,spt,j),Xkk11q(:,spt,j),MUq1(:,spt,j)]=fimm(spt,Rmeasure(3,3),X_jpda_cvk_1q(:,spt-1,j),P_jpda_cvk_1q(:,:,spt-1,j),MUq1(:,spt-1,j),T,Zcv_1q(:,spt,j),2);

        X_jpda_cvk_1(:,spt,j)=[X_jpda_cvk_1r(1,spt,j);X_jpda_cvk_1theta(1,spt,j);X_jpda_cvk_1q(1,spt,j)];
        X_jpda_cvkk1_1(:,spt,j)=[Xkk11r(1,spt,j);Xkk11theta(1,spt,j);Xkk11q(1,spt,j)];  % 预测直 
        S_jpda_1(:,:,spt,j)=diag([S1r(:,spt,j),S1theta(:,spt,j),S1q(:,spt,j)]);    
         K_jpda_1(:,:,spt,j)=[K1r(:,spt,j),Ktheta1(:,spt,j),K1q(:,spt,j)];
         
        track(j,2,spt)=X_jpda_cvk_1r(1,spt,j);
        track(j,3,spt)=X_jpda_cvk_1theta(1,spt,j);
        track(j,4,spt)=X_jpda_cvk_1q(1,spt,j);
        track(j,5,spt)=Xkk11r(1,spt,j);
        track(j,6,spt)=Xkk11theta(1,spt,j);
        track(j,7,spt)=Xkk11q(1,spt,j);
        
    end
 
    PG=1-eps;
    gama=1.1;
    falsealarmdensities=0.000000000000000001;
    %suppose the probability of dectation PD=0.99
    PD=0.99;
 if tracknum>=2
ZZ_jpda_1=ZZ_jpda(:,:,1);
ZZ_jpda_2=ZZ_jpda(:,:,2);
% X_jpda_cvk_1(:,i)=X_jpda_cvk_1(:,spt,1);
% X_jpda_cvk_2(:,i)=X_jpda_cvk_1(:,spt,2);
% S_jpda_1(:,:,i)=
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 计算单个目标jpda的计算量
    t=cputime; 
    clear omega ZZ;
    % 生成确认阵
    [omega,ZZ]=fform_validation_matrix(ZZ_jpda_1,ZZ_jpda_2);
  ZZ
    clear em ee;
    % 生成可行矩阵
    [em,ee]=fget_data_association_hypotheses(omega);
    clear tao sigma phi;
    % 计算 关联指示器\目标检测指示器 和 联合事件中假量测数量
    [tao,sigma,phi]=fcalculate_indicators(ee);
%     sigma
    %计算可行矩阵行、列和数量
    [n1,n2,n3]=size(ee);
% n1  
% n2   
% n3  
    clear sigma0
    sigma0=sigma;
     
    clear e V_jpda P_product_PD numerator PDF_measurements;
    for ii=1:n3     %%%%( n3=7 )
        product_Gause=1;
        for j=1:n1   %% 2
            e(j)=1;
%       em =
%      1     2     2     3     3     1     1
%      1     1     3     1     2     2     3
          if em(j,ii)==1
                e(j)=1;
             elseif em(j,ii)==2
                %V_jpda(:,j) is the innovation of the jth measurement
                V_jpda(:,j)=ZZ(:,j)-X_jpda_cvk_1(:,spt,1);
                e(j)=exp(-0.5*V_jpda(:,j)'*inv(S_jpda_1(:,:,spt,1))*V_jpda(:,j));
             elseif em(j,ii)==3
                V_jpda(:,j)=ZZ(:,j)-X_jpda_cvk_1(:,spt,2);
                e(j)=exp(-0.5*V_jpda(:,j)'*inv(S_jpda_1(:,:,spt,2))*V_jpda(:,j));
             end           
             product_Gause=product_Gause*e(j);
        end
        PDF_measurements(ii)=product_Gause;
        
        product_PD=1;