%用老方法濾波50次;stardard kalman filtering
for m=1:num
    PHI=   [1,T,0,0,0,0;   %PHI is the state transition matrix 
            0,1,0,0,0,0;
            0,0,1,T,0,0;
            0,0,0,1,0,0;
            0,0,0,0,1,T;
            0,0,0,0,0,1];
     Hk=   [1,0,0,0,0,0;   %Hk is the observed design matrix
               0,0,1,0,0,0;
               0,0,0,0,1,0];
     Rk=0.01*[1,0,0;0,1,0;0,0,1]; %covariance matrix of obsevation noise
     Pk=400*[1,0,0,0,0,0; %the covariance matrix of initial state vector "X0"  
             0,1,0,0,0,0;
             0,0,1,0,0,0;
             0,0,0,1,0,0;
             0,0,0,0,1,0;
             0,0,0,0,0,1];
     Qk=0.1*[1,0,0,0,0,0; %the covariance matrix of state noise;Wk的協方差矩陣
             0,1,0,0,0,0;
             0,0,1,0,0,0;
             0,0,0,1,0,0;
             0,0,0,0,1,0;
             0,0,0,0,0,1];
     vx=(zx(2)-zx(1))/T;%the velocity of x
     vy=(zy(2)-zy(1))/T;%the velocity of y
     vz=(zz(2)-zz(1))/T;%the velocity of z
     Xk=[zx(1);vx;zy(1);vy;zz(1);vz];%initial state vector at k epoch
        
    %濾波開始;filtering start
    for r=3:N;
        Zk=[zx(r);zy(r);zz(r)];
        
        FXk=PHI*Xk;            %Forecasting vector:FXk
        Mk=PHI*Pk*PHI'+Qk;     %calculating the coviriance matrix of forecasting vector
        Vk=Zk-Hk*FXk;          %get the innovation vector;  
        MVk=Hk*Mk*Hk'+Rk;      %get the covariance matrix of innovation vector
%==================for fading filtering====================================
%         Sk=trace(MVk-Hk*Qk*Hk'-Rk)/trace(Hk*PHI*Pk*PHI'*Hk');
%         if(Sk<=1)
%            lamda=1;
%         else
%            lamda=Sk;
%            Mk=lamda*PHI*Pk*PHI'+Qk;
%            
%         end;
%==========================================================================
        Kk=Mk*Hk'*inv(MVk);    %calculating the gain matrix
        Xk=FXk+Kk*Vk;          %get the new state estimation
        Pk=(eye(6)-Kk*Hk)*Mk;  %get the covariance matrix of new state
        
        Xks(r)=Xk(1,1);%get x's coordinate
        Yks(r)=Xk(3,1);%....y's........
        Zks(r)=Xk(5,1);%....z's......
        Vkxs(r)=Xk(2,1);%get the velocity of x's coordinate
        Vkys(r)=Xk(4,1);%....................y's...........
        Vkzs(r)=Xk(6,1);%....................z's...........
        rex(m,r)=Xks(r);%get the x's coordinate at "r" epoch in "m" times
        rey(m,r)=Yks(r);
        rez(m,r)=Zks(r);
    end %結束一次濾波
end


ex1=N:1;ey1=N:1;ez1=N:1;
%計算濾波的均值%計算濾波誤差的均值
for i=3:N
    ex=0;ey=0;ez=0;
    eqx=0;eqy=0;eqz=0;
    for j=1:num
        eqx=eqx+rex(j,i);%the x's sum of filter at "i" epoch;
        eqy=eqy+rey(j,i);%....y's..........................
        eqz=eqz+rez(j,i);%....z's...........................
        ex=ex+(rex(j,i)-x(i));%the sum of filter error of x at "i" epoch
        ey=ey+(rey(j,i)-y(i));%...........................y............
        ez=ez+(rez(j,i)-z(i));%...........................z............
    end
    ex1(i)=ex/num;%get the mean value of x's filter error
    ey1(i)=ey/num;%.......................y's............
    ez1(i)=ez/num;%.......................z's............
    eqx1(i)=eqx/num;%get the mean value of x's filter
    eqy1(i)=eqy/num;%......................y's........
    eqz1(i)=eqz/num;%......................z's........
end
%繪圖


 figure(1);
 axes1 = axes('FontSize',13,'FontWeight','bold');
 plot(ex1,'LineWidth',2);%,ex1,ex1,'r-');
 xlabel('\bfEpoch(s)','FontSize',12);ylabel('\bfX Error(m)','FontSize',12) % x軸注解
 grid(axes1,'on');
 
 figure(2);
 axes1 = axes('FontSize',13,'FontWeight','bold');
 plot(ey1,'LineWidth',2);%,ex1,ex1,'r-');
 xlabel('\bfEpoch(s)','FontSize',12);ylabel('\bfY Error(m)','FontSize',12) % y軸注解
 grid(axes1,'on');
 
  figure(3);
  axes1 = axes('FontSize',13,'FontWeight','bold');
  plot(ez1,'LineWidth',2);%,ex1,ex1,'r-');
  xlabel('\bfEpoch(s)','FontSize',12);ylabel('\bfZ Error(m)','FontSize',12) % z軸注解
  grid(axes1,'on');