trajectory3;

x(:,1)=X(:,1);  %初始狀態向量

R=diag([200^2  10^2  200^2  10^2]);
P=50*eye(6);   %初始狀態向量協方差


%CA
F=[1  T  T^2/2  0   0    0
   0  1    T    0   0    0
   0  0    1    0   0    0
   0  0    0    1   T  T^2/2
   0  0    0    0   1    T
   0  0    0    0   0    1];
  
Q=[T^5/20    T^4/8    T^3/6        0          0         0
   T^4/8     T^3/3    T^2/2        0          0         0
   T^3/6     T^2/2      T          0          0         0
     0         0        0       T^5/20      T^4/8     T^3/6
     0         0        0       T^4/8       T^3/3     T^2/2
     0         0        0       T^3/6       T^2/2       T    ]*5;
 
for i=2:length(z)    
     x(:,i)=F*x(:,i-1);
     P=F*P*F'+Q;
     H=[ 1, 0, 0, 0, 0, 0
         0, 1, 0, 0, 0, 0
         0, 0, 0, 1, 0, 0
         0, 0, 0, 0, 1, 0];
     K=P*H'*inv(H*P*H'+R);% 增益K
     V=z(:,i)-H*x(:,i);
     x(:,i)=x(:,i)+K*V;
     P=(eye(6)-K*H)*P*(eye(6)+K*H)'-K*R*K';
end

%畫圖觀察
% figure(1);
% plot(X(1,:),'b-'),hold on;
% plot(z(1,:),'k:')
% plot(x(1,:),'r:')
% axis square
% title('位移');
% xlabel('秒');ylabel('米');
% legend('真實軌跡','測量值','濾波值');
% 
% figure(2);
% plot(X(2,:),'b-'),hold on;
% plot(z(2,:),'k:')
% plot(x(2,:),'r:')
% axis square
% title('速度');
% xlabel('秒');ylabel('米/秒');
% legend('真實軌跡','測量值','濾波值');