com_imm.m

This paper studies the problem of tracking a ballistic object in the reentry phase by processing ra

function COM_imm
%**************************************************************************
%STATEMENT:
%    1.This expriment aim at comparing three smodel.The first improved 
%the model probility based on conventional imm model;the second improved
%innovatin and process noise covariance;the third combine above-mentioned
%two model.The performance is displayed.
%    2.This expriment have two sensor tracking one target.
%**************************************************************************

%given trajectory composed of true position and true velocity.
[y1 y2 T]=Exerc2;
ys1=y1+300*randn(size(y1)); %the first sensor measure;
%ys2+y1+50*randn(size(y1)); %the second sensoe measure;
ev=30; %the measure noise error standard;
ev2=ev*ev;
R=ev2*eye(3); %the measure noise covariance matrix;

t=T; %sample time;
L=size(y1,2); %the number of sample point;

err_1=zeros(6,L);
%err_2=zeros(6,L);
%err_3=zeros(6,L);

MC=50; %Monte Carlo;
g=9.8; %the gravity acceleration;
%a=0.05; %maneuvering frequency;

%Monte Carlo expriment;
%**************************************************************************
for mc=1:MC
    
    S1=zeros(9,L);
    P1=zeros(9,9,L);
    %S2=zeros(9,L);
    %P2=zeros(9,9,L);
    %S3=zeros(9,L);
    %P3=zeros(9,9,L);
    
    %the first model:improve the model probility;
    %**********************************************************************
    %step1:initialize;
    %sub-model1:constant velocity;
    ss0(1)=ys1(1,1);
    ss0(2)=ys1(2,1);
    ss0(3)=ys1(3,1);
    ss0(4)=(ys1(1,2)-ys1(1,1))/t;
    ss0(5)=(ys1(2,2)-ys1(2,1))/t;
    ss0(6)=(ys1(3,2)-ys1(3,1))/t;
    ss0(7)=0;
    ss0(8)=0;
    ss0(9)=0;
    s0(:,1)=ss0;
    ep2=ev2*t*t/4+ev2;
    pp0=[ev2 0 ev2/t 0;0 ev2 0 ev2/t;ev2/t 0 ep2 0;0 ev2/t 0 ep2];
    p0(:,:,1)=eye(9,4)*pp0*eye(4,9);
    %sub-model2:constant acceleration;
    ss0(7)=(ys1(1,3)+ys1(1,1)-2*ys1(1,2))/(2*t*t);
    ss0(8)=(ys1(2,3)+ys1(2,1)-2*ys1(2,2))/(2*t*t);
    ss0(9)=(ys1(3,3)+ys1(3,1)-2*ys1(3,2))/(2*t*t);
    ss0(4)=(ys1(1,2)-ys1(1,1))/t-ss0(7)*t;
    ss0(5)=(ys1(2,2)-ys1(2,1))/t-ss0(8)*t;
    ss0(6)=(ys1(3,2)-ys1(3,1))/t-ss0(9)*t;
    s0(:,2)=ss0;
    ep2=ev2*t*t/4+2*ev2/(t*t);
    ep3=2*ev2/(t*t);
    ep4=3*ev2/(t^3)+ev2*t*t/2;
    ep5=6*ev2/(t^4)+ev2*t*t/12;
    pp0=[ev2 0 0 ev2/t 0 0 ep3 0 0;
         0 ev2 0 0 ev2/t 0 0 ep3 0;
         0 0 ev2 0 0 ev2/t 0 0 ep3;
         ev2/t 0 0 ep2 0 0 ep4 0 0;
         0 ev2/t 0 0 ep2 0 0 ep4 0;
         0 0 ev2/t 0 0 ep2 0 0 ep4;
         ep3 0 0 ep4 0 0 ep5 0 0;
         0 ep3 0 0 ep4 0 0 ep5 0;
         0 0 ep3 0 0 ep4 0 0 ep5];
    p0(:,:,2)=pp0;
    %sub-model3:singer model;
    s0(:,3)=ss0;
    p0(:,:,3)=pp0;
    %sub-model4:current statistic model;
    s0(:,4)=ss0;
    p0(:,:,4)=pp0;
    
    p=[0.85 0.05 0.05 0.05;
       0.05 0.85 0.05 0.05;
       0.05 0.05 0.85 0.05;
       0.05 0.05 0.05 0.85];       %Markov transition probility;
   u0=[0.1 0.2 0.3 0.4]';         %initial probility;
   u=zeros(4,L);
   u(:,1)=u0;
   a=0.05;                        %manuver frequent;
   amax=4*g;
   
   for k=1:L
       %step 1:calculate weight;
       c=zeros(4,1);
       uu=zeros(4,4);
       if k==1
           for j=1:4
               c(j)=sum(p(:,j).*u0); %thr normalize constant;
           end
           for j=1:4
               for i=1:4
                   uu(i,j)=p(i,j)*u0(i)/c(j); %transition probility update;
               end
           end
       else
           for j=1:4
               c(j)=sum(p(:,j).*u(:,k-1));
           end
           for j=1:4
               for i=1:4
                   uu(i,j)=p(i,j)*u(i,k-1)/c(j);
               end
           end
       end
       
       %step 2:calculate filter input;
       mk0=zeros(9,9,4,4);
       mk=zeros(9,9,4,4);
       xprev2=zeros(9,4);
       pprev2=zeros(9,9,4);
       eA=zeros(9,9,4,4);
       for j=1:4
           if j==1
               [A,Q]=cvv(t);
           elseif j==2
               [A,Q]=caa(t);
           elseif j==3
               [A,Q]=sig(t);
           else
               [A,A1,Q1]=sigc(t);
           end
           if k==1
               if j==4
                   aprev=sqrt(s0(7,j)^2+s0(8,j)^2+s0(9,j)^2);
                   if s0(9,j)>0
                       q=(4-pi)/pi*(amax-aprev)^2;
                   else
                       q=(4-pi)/pi*(-amax-aprev)^2;
                   end
                   Q=2*a*q*q*Q1;
               end
               for i=1:4
                   mk(:,:,i,j)=p(i,j)/c(j)*A*p0(:,:,i)*A'+Q;
               end
           else
               if j==4
                   aprev=sqrt(S1(7,k-1)^2+S1(8,k-1)^2+S1(9,k-1)^2);
                   if S1(9,k-1)>0
                       q=(4-pi)/pi*(amax-aprev)^2;
                   else
                       q=(4-pi)/pi*(-amax-aprev)^2;
                   end
                   Q=2*a*q*Q1;
               end
               for i=1:4
                   mk(:,:,i,j)=p(i,j)/c(j)*A*tempp(:,:,i)*A'+Q; 
               end
           end
           for i=1:4
               mk0(:,:,j)=mk0(:,:,j)+uu(i,j)*inv(mk(:,:,i,j)); 
           end
           for i=1:4
               eA(:,:,i,j)=uu(i,j)*inv(mk0(:,:,j))*inv(mk(:,:,i,j))*A; %eA is the weight of each model;
           end



           if k==1
               xprev1=s0;
               pprev1=p0;
           else
               xprev1=temps;
               pprev1=tempp;
           end
           for i=1:4
               xprev2(:,j)=xprev2(:,j)+eA(:,:,i,j)*xprev1(:,i); %model interaction;
           end
           for i=1:4
               pprev2(:,:,j)=pprev2(:,:,j)+uu(i,j)*(pprev1(:,:,i)+...
                   (xprev1(:,i)-xprev2(:,j))*(xprev1(:,i)-xprev2(:,j))');  %covriance interaction;
           end
       end
           
        %step 3:Bank of kalman filter produce;
        temps=zeros(9,4);
        tempp=zeros(9,9,4);
        zk=zeros(4,1);
        for j=1:4
           if j==1
               [A,Q,C]=cvv(t);
           elseif j==2
               [A,Q,C]=caa(t);
           elseif j==3
               [A,Q,C]=sig(t);
           else
               [A,A1,Q1,C]=sigc(t);
           end
           
           if j<4
               xprev3=A*xprev2(:,j);                                       %one step state estimate;
           else
               xprev3=A1*xprev2(:,j);                                      %to current statistic model;
           end
           
           if j<4
               pprev3=A*pprev2(:,:,j)*A'+Q;                                %one step estimate covriance;
           elseif k>1
               aprev=sqrt(S1(7,k-1)^2+S1(8,k-1)^2+S1(9,k-1)^2);
                if S1(9,k-1)>0
                    q=(4-pi)/pi*(amax-aprev)^2;
                else
                    q=(4-pi)/pi*(-amax-aprev)^2;
                end
                Q=2*a*q*q*Q1;
                pprev3=A*pprev2(:,:,j)*A'+Q;
           end
           sk=C*pprev3*C'+R;        
           rk=ys1(:,k)-C*xprev3;                                             %innovation；
           kk=pprev3*C'*inv(sk);                                           %kalman filter gain；
           temps(:,j)=xprev3+kk*rk;
           tempp(:,:,j)=(eye(9)-kk*C)*pprev3*(eye(9)-kk*C)'+kk*R*kk';
           skd=abs(det(sk));
           zkk=rk'*inv(sk)*rk;
           zk(j)=exp(-zkk/2)/sqrt(2*pi*skd);                               %likehood function;
        end
        
        %step 4:transtion probility update;
        c1=0;
        if k>1
            for j=1:4
                c1=c1+zk(j)*sum(p(:,j).*u(:,k-1));
            end
            for j=1:4
                for i=1:4
                    u(j,k)=u(j,k)+zk(j)/c1*p(i,j)*u(i,k-1);                    %transfer probility update；
                end
            end
        end
        
        %step 5:the combined state estimate;
        ux=zeros(9,9);
        uux=zeros(9,9,4);
        for j=1:4
            ux=ux+u(j,k)*inv(tempp(:,:,j)); 
        end
        for j=1:4
            uux(:,:,j)=u(j,k)*inv(ux)*inv(tempp(:,:,j));
        end
        for j=1:4
            S1(:,k)=S1(:,k)+uux(:,:,j)*temps(:,j);
            P1(:,:,k)=P1(:,:,k)+u(j,k)*(tempp(:,:,j)+...
              (temps(:,j)-S1(:,k))*(temps(:,j)-S1(:,k))');
        end
   end
   
   err_1=err_1+sqrt((y2-S1([1 2 3 4 5 6],:)).^2)/MC;
end

er_1=sqrt(err_1(1,:).^2+err_1(2,:).^2+err_1(3,:).^2);

figure
plot3(S1(1,:),S1(2,:),S1(3,:))
grid on

figure
plot(err_1(1,:),'r')
hold on
plot(err_1(2,:),'b')
plot(err_1(3,:),'g')
legend('x-error','y-error','z-error')
title('position error')
set(gcf,'color','white')
set(gca,'Visible','on')

figure
plot(er_1)
    
figure 
plot(u(1,:),'r')
hold on
plot(u(2,:),'b')
plot(u(3,:),'g')
plot(u(4,:),'k')
legend('constant velocity','constant acceleration','singer model','current statistic')

%figure 
%grid on
%hold on
%view(-12,30)%(101,12)
%axis([0 4*10^5 -1*10^4 7*10^4 1*10^5 8*10^5])
%for i=1:L
%    plot3(y1(1,i),y1(2,i),y1(3,i),'b:^')
%    pause(0.05)
%    plot3(S1(1,i),S1(2,i),S1(3,i),'r-*')
%    pause(1)
%end