T=1;
X(:,1)=[120000 -426 0 2000 0 0];
for k=2:31   %cv
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],31)=[5 -10]';
for k=32:38   %ca
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],38)=[-8 18]';
for k=39:49   %ca
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],49)=[10 -20]';
for k=50:61   %ca
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],61)=[0 30]';
for k=62:65   %ca
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],65)=[-10 -8]';
X(:,66)=[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]*X(:,65);
X([3 6],66)=[-5 0]';
for k=67:81   %ca
    X(:,k)=[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]*X(:,k-1);
end
X([3 6],81)=[5 -10]';
for k=82:90   %ca
    X(:,k)=[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]*X(:,k-1);
end


Z(1,:)=X(1,:);%x位置
Z(2,:)=X(2,:);%x速度
Z(3,:)=X(4,:);%y位置
Z(4,:)=X(5,:);%y速度

delta_weiyi=200;     %位移誤差
delta_sudu=10;       %速度誤差
%加噪
z(1,:)=Z(1,:)+(2*rand(1,90/T)-1)*delta_weiyi;  %x測量位移
z(2,:)=Z(2,:)+(2*rand(1,90/T)-1)*delta_sudu;   %x測量速度
z(3,:)=Z(3,:)+(2*rand(1,90/T)-1)*delta_weiyi;  %y測量位移
z(4,:)=Z(4,:)+(2*rand(1,90/T)-1)*delta_sudu;   %y測量速度


%畫圖
% figure; %x位移
% plot(X(1,:),'k'); %真實
% hold on
% plot(z(1,:),'b:'); %測量
% title('X軸位移');
% xlabel('t/（秒）');ylabel('位移/（米）');
% legend('真實','測量');
% 
% figure; %x速度
% plot(X(2,:),'k'); %真實
% hold on
% plot(z(2,:),'b:'); %測量
% title('X軸速度');
% xlabel('t/（秒）');ylabel('速度/（米）');
% legend('真實','測量');
% 
% figure; %y位移
% plot(X(4,:),'k'); %真實
% hold on
% plot(z(3,:),'b:'); %測量
% title('y軸位移');
% xlabel('t/（秒）');ylabel('位移/（米）');
% legend('真實','測量');
% 
% figure; %y速度
% plot(X(5,:),'k'); %真實
% hold on
% plot(z(4,:),'b:'); %測量
% title('y軸速度');
% xlabel('t/（秒）');ylabel('速度/（米）');
% legend('真實','測量');
% 
% figure; %XY平面上的目標軌跡
% plot(X(1,:),X(4,:),'k'); %真實
% hold on
% plot(z(1,:),z(3,:),'b:'); %測量
% title('XY平面上的目標軌跡');
% xlabel('x/（米）');ylabel('y/（米）');
% legend('真實','測量');



%非線性
% Z(1,:)=sqrt(X(1,:).^2+X(4,:).^2);
% Z(2,:)=atan2(X(4,:),X(1,:));
% save data X Z;
% 
% figure
% plot(X(1,:),X(4,:));
% title('XY平面上的目標軌跡');
% xlabel('x/（米）');ylabel('y/（米）');