pn_guidance.asv

这是比例导引的MATLAB程序,其中龙格库塔积分的程序很简单且通用性强

%在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