%在Zarchan書的基礎上,05年12月10日,進行了一些修改.
%改成了公制單位,一些數據更適合于空地導彈上的使用.并加了一些注釋
%減小了運算的步長，發現對結果影響并不大
%比例導引也要用這個程序
%06年1月8日，進行了一些修改,只對固定目標的進行
%06年1月9日，進行了很大程度的修改，程序完全改頭換面。
%06年3月7日，在原來程序的基礎上修改，目前的程序，只用于比例導引,而且是針對固定目標的比例導引制導律。

clc
clear
global XNC VM;


RM1=0.; %導彈初始位置
RM2=500.;

RT1=5000.;%目標（或者假想目標點）初始位置
RT2=0.;
RTM1=RT1-RM1;
RTM2=RT2-RM2;


VM=260.;
Max_G=9.8*6;

XLAMF=-90/57.3; %命中目標時候導彈的攻擊角（impact angle）要求

theta=0; %導彈初始速度方向(角度)
VM1=VM*cos(theta);
VM2=VM*sin(theta);
      y(1)=theta;  
      y(2)=VM1;
      y(3)=VM2;
      y(4)=RM1;
      y(5)=RM2;  
T=0.;
T_step=.01; %仿真步長
n=1; %這個序號用于記錄數據用

VC=100; RTM=100; % 只是為了不為0,好編寫循環運算的代碼
%----------------------------------------
 while (VC >= 0)&(RTM>3)  
    
    RTM1=RT1-RM1;  
    RTM2=RT2-RM2;
    RTM=sqrt(RTM1^2+RTM2^2);  %彈目距離
    XLAM=atan2(RTM2,RTM1);%根據彈目線的連線，利用幾何關系來計算
    
    VTM1=-VM1;  %相對速度，Zarchan book P14
    VTM2=-VM2;
    VC=-(RTM1*VTM1+RTM2*VTM2)/RTM;
	XLAM_dot=(RTM1*VTM2-RTM2*VTM1)/(RTM*RTM);
         XNC=4.*VC*XLAM_dot;
 
%------過載限制------------------
        if XNC>Max_G
			XNC=Max_G;
		end
		if XNC<-Max_G
			XNC=-Max_G;
		end
%------過載限制代碼結束------------------        

%-------垂直于LOS使用這個代碼，同時右函數計算的，也需要更改------
%      AM1=-XNC*sin(XLAM);
%      AM2=XNC*cos(XLAM);
%--------------------------------------------------------------

     %調用子程序,進行一步龍格庫塔積分運算,解出彈道參數
      y=stepRK(T,y,T_step);
      theta=y(1);
      VM1=y(2);
      VM2=y(3);
      RM1=y(4);
      RM2=y(5);  
      
      
		ArrayT(n)=T;
		ArrayRM1(n)=RM1;
		ArrayRM2(n)=RM2;
		ArrayRT1(n)=RT1;
		ArrayRT2(n)=RT2;
		Array_theta_degree(n)=theta*57.3;
		XNCG=XNC/9.8; gravity_compensate=cos(theta);
        ArrayXNCG(n)=XNCG+gravity_compensate;%“重力補償”因素，也考慮進導彈的過載里面了。
        XLAM_degree=XLAM*57.3;
        ArrayXLAM_degree(n)=XLAM_degree; %這個值是負的。
  
 		ArraySeeker_degree(n)=(theta-XLAM)*57.3+XNCG*3; 
        % the angle between LOS and missile body(也就是導引頭的框架角)，把攻角因素也考慮了。        

   T=T+T_step;
   n=n+1;
end


% figure
% axis equal 
% plot(ArrayRM1,ArrayRM2),grid
% title('Engagement Geometry')
% xlabel('Downrange (m) ')
% ylabel('Altitude (m)')
% 
% figure
% plot(ArrayT,ArrayXNCG),grid
% title('Commanded Acceleration')
% xlabel('Time (Sec) ')
% ylabel('XNC (G)')
% 
% figure
% plot(ArrayT,ArrayXLAM_degree),grid
% title('Line-of-Sight Angle')
% xlabel('Time (Sec) ')
% ylabel('XLAM (Deg)')
% 
% figure
% plot(ArrayT,ArraySeeker_degree),grid
% title('seekr degree_ Angle')
% xlabel('Time (Sec) ')
% ylabel('seeker (Deg)')
disp '*** Simulation Complete'











% 
% figure
% plot(ArrayT,Array_seeker_angle),grid
% title('Seeker Angle')
% xlabel('Time (Sec) ')
% ylabel('seeker angle (Deg)')
% 
% 
% 
% figure
% plot(ArrayT,ArrayPsai_degree),grid
% title('Psai Angle')
% xlabel('Time (Sec) ')
% ylabel('Psai angle (Deg)')

%----output  Angles together --------
% figure
% plot(ArrayT,ArrayXLAM_degree,'blue',ArrayT,Array_theta_degree,'red',ArrayT,ArrayPsai_degree,'black'),grid
% title('Angles')
% xlabel('Time (Sec) ')
% ylabel('angles (Deg)')


% clc
% output=[ArrayT',ArrayRT1K',ArrayRT2K',ArrayRM1K',ArrayRM2K',ArrayXNCG',ArrayXLAM_degree'];
% save datfil.txt output /ascii

	
%plot(ArrayT,ArrayXLAM_degree,'blue',ArrayT,Array_theta_degree,'red',ArrayT,Array_seeker_angle,'magenta'),grid
%plot(ArrayT,ArrayXLAM_degree,'blue',ArrayT,Array_theta_degree,'red',ArrayT,ArrayPsai_degree,'black',ArrayT,Array_seeker_angle,'magenta'),grid