%左轉彎
function [X,Z,z]=turn_left(XX,t)
% 轉彎機動(飛機向原點逼近),XX=[x,vx,ax,y,vy,ay]',t為機動轉彎時間;
% 輸出[X,Z,z]分別為[xy真實值，極坐標真實值，測量值(加噪)];
% 飛機在轉彎時，其向心加速度一般為a=2g~3g左右，
% 且有公式：R=v*v/a=v/w,  a=w*v;  w為角速度，R為半徑，v為切向速度
% 取4維狀態向量X=(x vx ax  y vy ay)';

T=1;
w=0.08;     %角速度
v=sqrt(XX(2)^2+XX(5)^2);    %飛機的機動初合速度
alpha=atan2(XX(5),XX(2));   %飛機的機動初速度方向
a=XX(1)+v/w*cos(90*pi/180+alpha);  %圓心的坐標（a,b）;
b=XX(4)+v/w*sin(90*pi/180+alpha);

X(:,1)=[a+v/w*cos(w*T+alpha-90*pi/180)
              v*cos(w*T+alpha)
         (v*cos(w*T+alpha)-XX(2))/T
        b+v/w*sin(w*T+alpha-90*pi/180)
              v*sin(w*T+alpha)
         (v*sin(w*T+alpha)-XX(5))/T ];

for i=2:t/T
    X(1,i)=a+v/w*cos(w*i*T+alpha-90*pi/180);
    X(2,i)=v*cos(w*i*T+alpha);
    X(3,i)=(X(2,i)-X(2,i-1))/T;
    X(4,i)=b+v/w*sin(w*i*T+alpha-90*pi/180);
    X(5,i)=v*sin(w*i*T+alpha);
    X(6,i)=(X(5,i)-X(5,i-1))/T;
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,t)-1)*delta_weiyi;  %x測量位移
z(2,:)=Z(2,:)+(2*rand(1,t)-1)*delta_sudu;   %x測量速度
z(3,:)=Z(3,:)+(2*rand(1,t)-1)*delta_weiyi;  %y測量位移
z(4,:)=Z(4,:)+(2*rand(1,t)-1)*delta_sudu;   %y測量速度
% Z(1,:)=sqrt(X(1,:).^2+X(4,:).^2);   %轉為極坐標值Z
% Z(2,:)=atan2(X(4,:),X(1,:));
% 
% %極坐標值加噪z
% delta_r=50;     %徑向距離測量誤差協方差
% delta_a=0.005;  %方位角測量誤差的協方差
% z(1,:)=Z(1,:)+randn(1,t/T)*delta_r;    
% z(2,:)=Z(2,:)+randn(1,t/T)*delta_a;