clear
D(1)=10000;
b(1)=53/180*pi;
Km(1)=200/180*pi;
t(1)=0;


X1(1)=D(1)*sin(b(1));

Y1(1)=D(1)*cos(b(1));

T=0.1;

a0=19;
C=51329;
x1(1)=47169; 
M=2^16;  
for i=1:59999
	  x1(i+1)=mod(x1(i)*a0+C,M);
end
for i=1:60000
	  fakerand(i)=x1(i)/M;
end
for i=1:5000
	  normrnd1(i)=0;
	  for j=(12*(i-1)+1):(12*i)
		  normrnd1(i)=normrnd1(i)+fakerand(j);
      end
	  normrnd1(i)=normrnd1(i)-6;
end


Q1=120;
Q2=160;


for i=1:255    
   t(i+1)=t(1)+i*T; 
   K(1,1)=T*xiedistance(t(i),D(i),b(i),Km(i)); 
   K(2,1)=T*goallinew(t(i),D(i),b(i),Km(i));  
   K(3,1)=T*daodanvw(t(i),D(i),b(i),Km(i));  

   K(1,2)=T*xiedistance(t(i)+T/2,D(i)+K(1,1)/2,b(i)+K(2,1)/2,Km(i)+K(3,1)/2);
   K(2,2)=T*goallinew(t(i)+T/2,D(i)+K(1,1)/2,b(i)+K(2,1)/2,Km(i)+K(3,1)/2);
   K(3,2)=T*daodanvw(t(i)+T/2,D(i)+K(1,1)/2,b(i)+K(2,1)/2,Km(i)+K(3,1)/2);
       
          
    
   K(1,3)=T*xiedistance(t(i)+T/2,D(i)+K(1,2)/2,b(i)+K(2,2)/2,Km(i)+K(3,2)/2);
   K(2,3)=T*goallinew(t(i)+T/2,D(i)+K(1,2)/2,b(i)+K(2,2)/2,Km(i)+K(3,2)/2);
   K(3,3)=T*daodanvw(t(i)+T/2,D(i)+K(1,2)/2,b(i)+K(2,2)/2,Km(i)+K(3,2)/2);
         

   K(1,4)=T*xiedistance(t(i)+T,D(i)+K(1,3),b(i)+K(2,3),Km(i)+K(3,3));
   K(2,4)=T*goallinew(t(i)+T,D(i)+K(1,3),b(i)+K(2,3),Km(i)+K(3,3));
   K(3,4)=T*daodanvw(t(i)+T,D(i)+K(1,3),b(i)+K(2,3),Km(i)+K(3,3));
       
          
          
   D(i+1)=D(i)+(K(1,1)+2*K(1,2)+2*K(1,3)+K(1,4))/6;  

   b(i+1)=b(i)+(K(2,1)+2*K(2,2)+2*K(2,3)+K(2,4))/6;  

   Km(i+1)=Km(i)+(K(3,1)+2*K(3,2)+2*K(3,3)+K(3,4))/6;
end
% for i=1:250
%     x(i)=D(i)*sin(b(i))+Q1*normrnd1(i);
%     y(i)=D(i)*cos(b(i))+Q2*normrnd1(i);
%     x2(i)=D(i)*sin(b(i));
%     y2(i)=D(i)*cos(b(i));
% end
% plot(x,y,'b-',x2,y2,'r:')


X(1)=X1(1);
Y(1)=Y1(1);
Vm=400;
for i=1:255
    X1(i+1)=Vm*sin(Km(i))*T+X1(i);
    Y1(i+1)=Vm*cos(Km(i))*T+Y1(i);
    X(i+1)=Vm*sin(Km(i))*T+X1(i)+Q1*normrnd1(i);
    Y(i+1)=Vm*cos(Km(i))*T+Y1(i)+Q2*normrnd1(i);
end
plot(X,Y,'g*',X1,Y1,'b:')

Vw=20;
Kw=300/180*pi;
for i=0:150
    x3(i+1)=Vw*sin(Kw)*i*T;
    y3(i+1)=Vw*cos(Kw)*i*T;
end
hold on
plot(x3,y3,'r--')


legend('方差為Q1=120;Q2=160的觀測曲線','理論曲線');
title('導引系數為5時的比例導引運動的軌跡示意圖');
gtext('導彈的運動軌跡');
gtext('目標艦艇的運動軌跡');
xlabel('X坐標軸');
ylabel('Y坐標軸');
grid on