clear
D(1)=4000;
b(1)=60;
Km(1)=45;
t(1)=0;

T=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=2000;
Q2=3000;

for i=1:2500     
   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:2500
%     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:')



Vm=600;
for i=1:2500
    X(i)=Vm*sin(Km(i))*i*T+Q1*normrnd1(i);
    Y(i)=Vm*cos(Km(i))*i*T+Q2*normrnd1(i);
    X1(i)=Vm*sin(Km(i))*i*T;
    Y1(i)=Vm*cos(Km(i))*i*T;
end
Kw=60;
Vw=30;
for i=1:5000
    x3(i)=Vw*sin(Kw)*i*T;
    x3(i)=Vw*cos(Kw)*i*T;
end
hold on
plot(X,Y,'r',X1,Y1,'g--',x3,x3,'b--')


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