cs.m

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

function CS

%current statistic model;

[y1,y2,T]=Exerc5; %true position;
ev=300;
ev2=ev*ev;
R=ev2*eye(3); %measure covriance;
t=T;

a=0.05; %maneuver frequent;
a1=(-1+a*t+exp(-a*t))/(a*a);
a2=(1-exp(-a*t))/a;
a3=exp(-a*t);
A=[1 0 0 t 0 0 a1 0 0;
    0 1 0 0 t 0 0 a1 0;
    0 0 1 0 0 t 0 0 a1;
    0 0 0 1 0 0 a2 0 0;
    0 0 0 0 1 0 0 a2 0;
    0 0 0 0 0 1 0 0 a2;
    0 0 0 0 0 0 a3 0 0;
    0 0 0 0 0 0 0 a3 0;
    0 0 0 0 0 0 0 0 a3];
A1=[1 0 0 t 0 0 t*t/2 0 0;
    0 1 0 0 t 0 0 t*t/2 0;
    0 0 1 0 0 t 0 0 t*t/2;
    0 0 0 1 0 0 t 0 0;
    0 0 0 0 1 0 0 t 0;
    0 0 0 0 0 1 0 0 t;
    0 0 0 0 0 0 1 0 0;
    0 0 0 0 0 0 0 1 0;
    0 0 0 0 0 0 0 0 1];
%q=1;  %q为系统均方差；
q11=(1-exp(-2*a*t)+2*a*t+2*a^3*t^3/3-2*a^2*t^2-4*a*t*exp(-a*t))/(2*a^5);
q12=(exp(-2*a*t)+1-2*exp(-a*t)+2*a*t*exp(-a*t)-2*a*t+a^2*t^2)/(2*a^4);
q13=(1-exp(-2*a*t)-2*a*t*exp(-a*t))/(2*a^3);
q22=(4*exp(-a*t)-3-exp(-2*a*t)+2*a*t)/(2*a^3);
q23=(exp(-2*a*t)+1-2*exp(-a*t))/(2*a*a);
q33=(1-exp(-2*a*t))/(2*a);
Q1=[q11 0 0 q12 0 0 q13 0 0;
    0 q11 0 0 q12 0 0 q13 0;
    0 0 q11 0 0 q12 0 0 q13;
    q12 0 0 q22 0 0 q23 0 0;
    0 q12 0 0 q22 0 0 q23 0;
    0 0 q12 0 0 q22 0 0 q23;
    q13 0 0 q23 0 0 q33 0 0;
    0 q13 0 0 q23 0 0 q33 0;
    0 0 q13 0 0 q23 0 0 q33];
%Q=2*q*q*Q1;
H=[1 0 0 0 0 0 0 0 0;
   0 1 0 0 0 0 0 0 0;
   0 0 1 0 0 0 0 0 0];


L=size(y1,2);
S=zeros(9,L); %estimate state;
P=zeros(9,9,L); %estimate covariance;
g=9.8;
amax=4*g;


   
%analyze;
err=zeros(3,L);
errv=zeros(3,L);
err1=zeros(6,L);

mc=100; %Monte Carlo;

for m=1:mc
    
y=y1+300*randn(size(y1));%measure position;

%initialize;
s0(1)=y(1,1);
s0(2)=y(2,1);
s0(3)=y(3,1);
s0(4)=(y(1,2)-y(1,1))/T;
s0(5)=(y(2,2)-y(2,1))/T;
s0(6)=(y(3,2)-y(3,1))/T;
s0(7)=(y(1,3)+y(1,2)-2*y(1,2))/(2*T*T);
s0(8)=(y(2,3)+y(2,1)-2*y(2,2))/(2*T*T);
s0(9)=(y(3,3)+y(3,1)-2*y(3,2))/(2*T*T);
S(:,1)=s0;
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;
p0=[   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];
P(:,:,1)=p0;

for k=2:L
    xprev=S(:,k-1);
    aprev=sqrt(xprev(7)^2+xprev(8)^2+xprev(9)^2);
    if xprev(9)>0
        q=(4-pi)/pi*(amax-aprev)^2;
    else
        q=(4-pi)/pi*(-amax-aprev)^2;
    end
    Q=2*a*q*q*Q1;  
    
    %Kalman Filter;
    pprev=P(:,:,k-1);
    ppred=A*pprev*A'+Q; %one step estimate covriance;
    sk=H*ppred*H'+R;
    kk=ppred*H'*inv(sk); %filter gain;
    xpred=A1*xprev; %one step estimate state;
    rk=y(:,k)-H*xpred; %innovation;
    xpre=xpred+kk*rk; %filter state estimate;
    ppre=(eye(9)-kk*H)*ppred*(eye(9)-kk*H)'+kk*R*kk'; %filter estimate covariance;
    
    S(:,k)=xpre;
    P(:,:,k)=ppre;
end

cont=2;
S1=zeros(9,L);
P1=zeros(9,9,L);
S1(:,1)=s0;
P1(:,:,1)=p0;
cov=zeros(3,1);

for k=1:L
    if k==1
        xprev=S1(:,1);
        pprev=P1(:,:,1);
    else
        xprev=temps;
        pprev=tempp;
    end;
    xprev1=A1*xprev;
    if k>1
        for i=1:3
            cov(i)=1-exp(-1*(xprev1(i)-y(i,k))^2/(cont*abs(sk(i,i))+eps));
        end
        Q=2*a*([cov;cov;cov]*ones(1,9)).*Q1;
    else
        Q=2*a*Q1;
    end
    ppred=A*pprev*A'+Q;
    sk=H*ppred*H'+R;
    kk=ppred*H'*inv(sk);
    rk=y(:,k)-H*xpred;
    temps=xpred+kk*rk;
    tempp=(eye(9)-kk*H)*ppred*(eye(9)-kk*H)'+kk*R*kk';
    
    S1(:,k)=temps;
    P1(:,:,k)=tempp;
end

err(1,:)=err(1,:)+sqrt((y1(1,:)-S(1,:)).^2)/mc;
err(2,:)=err(2,:)+sqrt((y1(2,:)-S(2,:)).^2)/mc;
err(3,:)=err(3,:)+sqrt((y1(3,:)-S(3,:)).^2)/mc;

errv(1,:)=errv(1,:)+sqrt((y2(4,:)-S(4,:)).^2)/mc;
errv(2,:)=errv(2,:)+sqrt((y2(5,:)-S(5,:)).^2)/mc;
errv(3,:)=errv(3,:)+sqrt((y2(6,:)-S(6,:)).^2)/mc;

err1=err1+sqrt((y2-S1([1 2 3 4 5 6],:)).^2)/mc;
end

%analyze;
figure 
plot3(y1(1,:),y1(2,:),y1(3,:),'r.')
hold on
plot3(S(1,:),S(2,:),S(3,:),'b')
grid on
hold off

figure
plot(err(1,:),'r')
hold on
plot(err(2,:),'b')
plot(err(3,:),'g')
legend('x error','y error','z error')
title('position error')
hold off
set(gcf,'Color','White');

figure
plot(errv(1,:),'r-.')
hold on
plot(errv(2,:),'b-.')
plot(errv(3,:),'g-.')
legend('x error','y error','z error')
title('velocity error')
hold off
%set(gca,'FontSize',12);
set(gcf,'Color','White')

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

figure 
plot(err1(4,:),'r.')
hold on
plot(err1(5,:),'b.')
plot(err1(6,:),'g.')
legend('x-error','y-error','z-error')
title('velocity error')
set(gcf,'color','white')