grkt2f.m

铅垂平面比利导引法弹道仿真程序

%龙格库塔右端函数，dy:导数值；y:积分值

function dy=grkt2f(y)

global AJZ;
global AJZT;
global AMa;
global Aa;
global ACX;
global ACY;
global AP;
global APT;
global AXG;
global AXGT;
global AMZWZ1;
global AMZWZ2;
global AMZAF;
global vt;
global q0;
global s0;
global K;
global ammax;

RHO=1.225;
SONIC=343.13;
AE=0.01745329;   		%角度化弧度
s=0.0227;             %特征面积
q=0.5*RHO*y(1)*y(1);          %动压头
Ma=y(1)/SONIC;          %计算马赫数
L=1.8;                  %特征长度
B=[2.1126 44.0832];      %两级发动机结束工作时间      
g=9.81;




alpha=y(4)-y(2);
ABS_alpha=abs(alpha);
%插值部分开始
P=interp1(APT,AP,y(8))*g;
CX=twointerp(Aa,AMa,ACX,ABS_alpha,Ma);
CY=twointerp(Aa,AMa,ACX,ABS_alpha,Ma);

if alpha<0
    CY=-CY;
end
XF=CX*q*s;
YF=CY*q*s;
%ny=(P*sin(alpha)+YF)/(y(7)*g),
wzt=y(3)*L/y(1);

if y(8)<=B(1) 
    MC=2.362;
else  if y(8)>B(2) 
         MC=0;
         P=0;
      else 
         MC=0.21059;
      end
  end
  
if y(8)<B(2)
XG=interp1(AXGT,AXG,y(8));  %插值重心
end

if y(8)<B(2)
  aa=twointerp(Aa,AMa,AMZWZ1,ABS_alpha,Ma);
  bb=twointerp(Aa,AMa,AMZWZ2,ABS_alpha,Ma);
  ab=[aa,bb];
  a=[.9831,.8896];
  MZWZ=interp1(a,ab,XG);
else
    MZWZ=twointerp(Aa,AMa,AMZWZ2,ABS_alpha,Ma);
end

MZAF=twointerp(Aa,AMa,AMZAF,ABS_alpha,Ma); 
if ABS_alpha>0
    MZAF=MZAF*57.3+CY*(XG-0.9381)/(L*alpha); %由于重心的改变，对MZAF进行修正
end
%插值部分结束
am=y(1)*K*(-vt*sin(y(10))+y(1)*sin(K*q0-s0-(K-1)*y(10)))/y(9);
if abs(am)>=ammax
    am=sign(am)*ammax;
end


JZ=interp1(AJZT,AJZ,y(8));
dy(1)=(P*cos(alpha)-XF-9.81*y(7)*sin(y(2)))/y(7);
dy(2)=(P*sin(alpha)+YF-9.81*y(7)*cos(y(2)))/(y(7)*y(1));
dy(3)=(MZAF*alpha+MZWZ*wzt)*q*s*L/JZ;
dy(4)=y(3);
dy(5)=y(1)*cos(y(2));
dy(6)=y(1)*sin(y(2));
dy(7)=-MC;
dy(8)=1;
dy(9)=vt*cos(y(10))-y(1)*cos(K*q0-s0-(K-1)*y(10));
dy(10)=am/(y(1)*K);