function EKF

[y1 y2 T]=Exerc;
t=T;
g=9.8;
a=0.5;
L=size(y1,2);
bet=4000;

A=[1 0 0 t 0 0
   0 1 0 0 t 0 
   0 0 1 0 0 t
   0 0 0 1 0 0 
   0 0 0 0 1 0
   0 0 0 0 0 1];
a1=(-1+a*t+exp(-a*t))/(a*a);
a2=(1-exp(-a*t))/a;
G=[a1 0 0 
   0 a1 0
   0 0 a1
   a2 0 0
   0 a2 0 
   0 0 a2];
G1=[t*t/2 0 0
    0 t*t/2 0 
    0 0 t*t/2
    t 0 0
    0 t 0
    0 0 t];
H=[1 0 0 0 0 0 
   0 1 0 0 0 0
   0 0 1 0 0 0];
U=[0 0 -g]';
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);
q22=(4*exp(-a*t)-3-exp(-2*a*t)+2*a*t)/(2*a^3);
Q1=[q11 0 0 q12 0 0 
    0 q11 0 0 q12 0 
    0 0 q11 0 0 q12
    q12 0 0 q22 0 0 
    0 q12 0 0 q22 0
    0 0 q12 0 0 q22];
Q=2*a*Q1;
err=zeros(6,L);
err1=zeros(6,L);

MC=100; 

for mc=1:MC
    %measure model;
    y=y1+300*randn(size(y1));
    ev=30;
    ev2=ev*ev;
    R=ev2*eye(3);
    
    S=zeros(6,L);
    P=zeros(6,6,L);
    S1=zeros(6,L);
    P1=zeros(6,6,L);
    %initialize;
    ss0(1)=y(1,1);
    ss0(2)=y(2,1);
    ss0(3)=y(3,1);
    ss0(4)=(y(1,2)-y(1,1))/t;
    ss0(5)=(y(2,2)-y(2,1))/t;%什么意思
    ss0(6)=(y(3,2)-y(3,1))/t;
    s0=ss0;
    ep2=ev2*t*t/4+2*ev2/(t*t);
    p0=[ev2 0 0 ev2/t 0 0
        0 ev2 0 0 ev2/t 0
        0 0 ev2 0 0 ev2/t
        ev2/t 0 0 ep2 0 0
        0 ev2/t 0 0 ep2 0 
        0 0 ev2/t 0 0 ep2];
    S(:,1)=s0;
    P(:,:,1)=p0;
    S1(:,1)=ss0;
    P1(:,:,1)=p0;
    for k=2:L
        %compute the process model Jacobian;
        if k==1;
            if S(4,k)<9144
                c1=1.227;
                c2=1.0931^-4;
            else
                c1=1.754;
                c2=1.491^-4;
            end
            air=c1*exp(-c2*s0(3));
            temp1=sqrt(s0(4)^2+s0(5)^2+s0(6)^2);
            temp2=[s0(4) s0(5) s0(6)]';
            fk=-0.5*g/bet*air*s0(3)*temp1*temp2;
            Fk(1,1)=0;
            Fk(1,2)=0;
            Fk(1,3)=0.5*g/bet*c2*air*s0(4)*temp1;
            FK(1,4)=-0.5*g/bet*air*(s0(4)+temp1^2)/temp1;
            Fk(1,5)=-0.5*g/bet*air*s0(4)*s0(5)/temp1;
            Fk(1,6)=-0.5*g/bet*air*s0(4)*s0(6)/temp1;
            Fk(2,1)=0;
            Fk(2,2)=0;
            Fk(2,3)=0.5*g/bet*c2*air*s0(5)*temp1;
            Fk(2,4)=Fk(1,5);
            Fk(2,5)=-0.5*g/bet*air*(s0(5)^2+temp1^2)/temp1;
            Fk(2,6)=-0.5*g/bet*air*s0(5)*s0(6)/temp1;
            Fk(3,1)=0;
            Fk(3,2)=0;
            Fk(3,3)=0.5*g/bet*c2*air*s0(6)*temp1;
            Fk(3,4)=Fk(1,6);%原出處
            Fk(3,5)=Fk(2,6);
            Fk(3,6)=-0.5*g/bet*air*(s0(6)^2+temp1^2)/temp1;
        else
            if S(4,k-1)<9144
                c1=1.227;
                c2=1.0931^-4;
            else
                c1=1.754;
                c2=1.491^-4;
            end
            s0=S(:,k-1);
            air=c1*exp(-c2*s0(3));
            temp1=sqrt(s0(4)^2+s0(5)^2+s0(6)^2);
            temp2=[s0(4) s0(5) s0(6)]';
            fk=-0.5*g/bet*air*s0(3)*temp1*temp2;
            Fk(1,1)=0;
            Fk(1,2)=0;
            Fk(1,3)=0.5*g/bet*c2*air*s0(4)*temp1;
            FK(1,4)=-0.5*g/bet*air*(s0(4)+temp1^2)/temp1;
            Fk(1,5)=-0.5*g/bet*air*s0(4)*s0(5)/temp1;
            Fk(1,6)=-0.5*g/bet*air*s0(4)*s0(6)/temp1;
            Fk(2,1)=0;
            Fk(2,2)=0;
            Fk(2,3)=0.5*g/bet*c2*air*s0(5)*temp1;
            Fk(2,4)=Fk(1,5);
            Fk(2,5)=-0.5*g/bet*air*(s0(5)^2+temp1^2)/temp1;
            Fk(2,6)=-0.5*g/bet*air*s0(5)*s0(6)/temp1;
            Fk(3,1)=0;
            Fk(3,2)=0;
            Fk(3,3)=0.5*g/bet*c2*air*s0(6)*temp1;
            Fk(3,4)=Fk(1,6);
            Fk(3,5)=Fk(2,6);
            Fk(3,6)=-0.5*g/bet*air*(s0(6)^2+temp1^2)/temp1;
        end
        if k==1
            xprev1=A*S(:,k)+G*(U+fk);
            pprev1=(A+G*Fk)*P(:,:,k)*(A+G*Fk)'+Q;
        else
            xprev1=A*S(:,k-1)+G*(U+fk);
            pprev1=(A+G*Fk)*P(:,:,k-1)*(A+G*Fk)'+Q;
        end
        rk=y(:,k)-H*xprev1;
        sk=H*pprev1*H'+R;
        kk=pprev1*H'*inv(sk);
        xprev2=xprev1+kk*rk;
        pprev2=(eye(6)-kk*H)*pprev1;
        
        S(:,k)=xprev2;
        P(:,:,k)=pprev2;
    end
    
    s0=ss0;
    for k=2:L
        %compute the process model Jacobian;
        if k==1;
            if S1(4,k)<9144
                c1=1.227;
                c2=1.0931^-4;
            else
                c1=1.754;
                c2=1.491^-4;
            end
            air=c1*exp(-c2*s0(3));
            temp1=sqrt(s0(4)^2+s0(5)^2+s0(6)^2);
            temp2=[s0(4) s0(5) s0(6)]';
            fk=-0.5*g/bet*air*s0(3)*temp1*temp2;
            Fk(1,1)=0;
            Fk(1,2)=0;
            Fk(1,3)=0.5*g/bet*c2*air*s0(4)*temp1;
            FK(1,4)=-0.5*g/bet*air*(s0(4)+temp1^2)/temp1;
            Fk(1,5)=-0.5*g/bet*air*s0(4)*s0(5)/temp1;
            Fk(1,6)=-0.5*g/bet*air*s0(4)*s0(6)/temp1;
            Fk(2,1)=0;
            Fk(2,2)=0;
            Fk(2,3)=0.5*g/bet*c2*air*s0(5)*temp1;
            Fk(2,4)=Fk(1,5);
            Fk(2,5)=-0.5*g/bet*air*(s0(5)^2+temp1^2)/temp1;
            Fk(2,6)=-0.5*g/bet*air*s0(5)*s0(6)/temp1;
            Fk(3,1)=0;
            Fk(3,2)=0;
            Fk(3,3)=0.5*g/bet*c2*air*s0(6)*temp1;
            Fk(3,4)=Fk(1,6);
            Fk(3,5)=Fk(2,6);
            Fk(3,6)=-0.5*g/bet*air*(s0(6)^2+temp1^2)/temp1;
        else
            if S1(4,k-1)<9144
                c1=1.227;
                c2=1.0931^-4;
            else
                c1=1.754;
                c2=1.491^-4;
            end
            s0=S1(:,k-1);
            air=c1*exp(-c2*s0(3));
            temp1=sqrt(s0(4)^2+s0(5)^2+s0(6)^2);
            temp2=[s0(4) s0(5) s0(6)]';
            fk=-0.5*g/bet*air*s0(3)*temp1*temp2;
            Fk(1,1)=0;
            Fk(1,2)=0;
            Fk(1,3)=0.5*g/bet*c2*air*s0(4)*temp1;
            FK(1,4)=-0.5*g/bet*air*(s0(4)+temp1^2)/temp1;
            Fk(1,5)=-0.5*g/bet*air*s0(4)*s0(5)/temp1;
            Fk(1,6)=-0.5*g/bet*air*s0(4)*s0(6)/temp1;
            Fk(2,1)=0;
            Fk(2,2)=0;
            Fk(2,3)=0.5*g/bet*c2*air*s0(5)*temp1;
            Fk(2,4)=Fk(1,5);
            Fk(2,5)=-0.5*g/bet*air*(s0(5)^2+temp1^2)/temp1;
            Fk(2,6)=-0.5*g/bet*air*s0(5)*s0(6)/temp1;
            Fk(3,1)=0;
            Fk(3,2)=0;
            Fk(3,3)=0.5*g/bet*c2*air*s0(6)*temp1;
            Fk(3,4)=Fk(1,6);
            Fk(3,5)=Fk(2,6);
            Fk(3,6)=-0.5*g/bet*air*(s0(6)^2+temp1^2)/temp1;
        end
        if k==1
            xprev1=A*S1(:,k)+G1*(U+fk);
            pprev1=(AG*Fk)*P1(:,:,k)*(A+G*Fk)'+Q;
        else
            xprev1=A*S1(:,k-1)+G1*(U+fk);
            pprev1=(A+G*Fk)*P1(:,:,k-1)*(A+G*Fk)'+Q;
        end
        rk=y(:,k)-H*xprev1;
        sk=H*pprev1*H'+R;%卡爾滿
        kk=pprev1*H'*inv(sk);
        xprev2=xprev1+kk*rk;
        pprev2=(eye(6)-kk*H)*pprev1;
        
        S1(:,k)=xprev2;
        P1(:,:,k)=pprev2;
    end
    
    err=err+sqrt((S-y2).^2)/MC;
    err1=err1+sqrt((S1-y2).^2)/MC;

end       
figure
plot3(y1(1,:),y1(2,:),y1(3,:),'r-.')
hold on
plot3(S(1,:),S(2,:),S(3,:),'b')
grid on

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')
set(gcf,'color','white')

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

figure
hold on
plot(err(1,:),'r-^')
plot(err1(1,:),'b:o')
legend('singer','currevt')
title('x-position error')
set(gcf,'color','white')

figure
hold on
plot(err(2,:),'r-^')
plot(err1(2,:),'b:o')
legend('singer','currevt')
title('y-position error')
set(gcf,'color','white')

figure
hold on
plot(err(3,:),'r-^')
plot(err1(3,:),'b:o')
legend('singer','currevt')
title('3-position error')
set(gcf,'color','white')

figure
hold on
plot(err(4,:),'r-^')
plot(err1(4,:),'b:o')
legend('singer','current')
title('x-velocity error')
set(gcf,'color','white')

figure
hold on
plot(err(5,:),'r-^')
plot(err1(5,:),'b:o')
legend('singer','current')
title('y-velocity error')
set(gcf,'color','white')

figure
hold on
plot(err(6,:),'r-^')
plot(err1(6,:),'b:o')
legend('singer','current')
title('z-velocity error')
set(gcf,'color','white')