clear
%目標先作勻速運動，然后作蛇形機動，再作勻加速運動，再作轉彎運動，最后作勻速運動
%產生[0 1]間的正態分布的隨機數
a0=19;
C=51329;
x1(1)=47169; 
M=2^16;  
for i=1:5999
	  x1(i+1)=mod(x1(i)*a0+C,M);
end
for i=1:6000
	  fakerand(i)=x1(i)/M;
end
for i=1:500
	  normrnd1(i)=0;
	  for j=(12*(i-1)+1):(12*i)
		  normrnd1(i)=normrnd1(i)+fakerand(j);
      end
	  normrnd1(i)=normrnd1(i)-6;
end


%目標的參數，初始位置
x0=3000;
y0=4000;

v=20;
T=0.1;
Q1=20;
Q2=30;

b=pi/4;

vx=v*sin(b);
vy=v*cos(b);

for i=1:50
    x2(i)=x0+vx*(i-1)*T;
    y2(i)=y0+vy*(i-1)*T;
end
plot(x2,y2,'r-')
hold on
plot(x2+Q1*normrnd1(1:50),y2+Q2*normrnd1(1:50),'g:')
w=[0.05,-0.05]
T=0.5;
x(1,1)=x2(50);
x(2,1)=vx;
x(3,1)=y2(50);
x(4,1)=vy;
a=200*randn(1,50*length(w));
b=300*randn(1,50*length(w));
for j=1:length(w)
    A=[1,sin(w(j)*T)/w(j),0,(cos(w(j)*T)-1)/w(j);0,cos(w(j)*T),0,-sin(w(j)*T);0,-(cos(w(j)*T)-1)/w(j),1,sin(w(j)*T)/w(j);0 ,sin(w(j)*T),0,cos(w(j)*T)];
    for i=(50*(j-1)+1):(50*j)
       x(:,i+1)=A*x(:,i);
    end
end
hold on
plot(x0,y0,'r^',x(1,1:50),x(3,1:50),'r-')
hold on
plot(x(1,1:50)+Q1*normrnd1(1:50),x(3,1:50)+Q2*normrnd1(1:50),'g:')
hold on
plot(x(1,50:100),x(3,50:100),'b-')
hold on
plot(x(1,50:100)+Q1*normrnd1(1:51),x(3,50:100)+Q2*normrnd1(1:51),'g:')

x0=x(1,100);
y0=x(3,100);


v=50;


a=30;
b=pi/4;

vx=v*sin(b);
vy=v*cos(b);

ax=a*sin(b);
ay=a*cos(b);


for i=1:10
    X1(i)=x0+vx*(i-1)*T+1/2*ax*((i-1)*T)^2;
    Y1(i)=y0+vy*((i-1)*T)+1/2*ay*((i-1)*T)^2;
end
hold on
plot(X1,Y1,'r:')
hold on
plot(X1+Q1*normrnd1(1:10),Y1+Q2*normrnd1(1:10),'g:')
gtext('起始點');
gtext('勻速運動段');
gtext('蛇形機動段');
gtext('勻加速運動段');
hold on
legend('理論曲線','方差為Q1=120;Q2=160的觀測曲線','起始點');
title('一般機動目標運動軌跡示意圖');

xlabel('X坐標軸');
ylabel('Y坐標軸');
grid on