clear?all;
close?all;
clc;

Time=0;
TimeStep=0.005;?%仿真步長
%-------初始化制導系統參數---------
pi=3.141592653;
g=9.8;
Ma=340;
%---------導彈參數--------------
Rmx=50;Rmy=0;?%導彈初始位置
Vm=-3.24*Time^3+41.7*Time^2-248*Time+888;?%導彈速度（m/s）
AmMay=500*g;?%導彈最大機動能力
HeadError=-0.001;?%指向角誤差

%-------目標參數------------
Rt=6000;??%目標初始距離（彈目斜距）
ThetaT=pi+8/180*pi;?%目標速度方向
Rtx=Rt*cos（8/180*pi）;?%目標初始X軸位置
Rty=Rt*sin（8/180*pi）;?%目標初始Y軸位置
Vt=5*Ma;?%目標速度（m/s）
AtMay=0*g;?%目標機動能力

%--------導彈和目標幾何運動學結算--------
Rtmx=Rtx-Rmx;?%彈目在X軸方向的相對距離
Rtmy=Rty-Rmy;?%彈目在Y軸方向的相對距離
Rtm=sqrt（Rtmx^2+Rtmy^2）;?%計算彈目相對距離
SightAngle=atan（Rtmy/Rtmx）;?%視線角
LeadAngle=asin（Vt*sin（SightAngle-ThetaT）/Vm）;?%指向角
Vmx=Vm*cos（SightAngle-LeadAngle+HeadError）;?%導彈沿X軸方向的速度分量
Vmy=Vm*sin（SightAngle-LeadAngle+HeadError）;?%導彈沿Y軸方向的速度分量
Vtx=Vt*cos（ThetaT）;?%目標沿X軸方向的速度分量
Vty=Vt*sin（ThetaT）;?%目標沿Y軸方向的速度分量
Vtmx=Vtx-Vmx;?%彈目在X軸方向上的相對速度
Vtmy=Vty-Vmy;?%彈目在Y軸上的相對速度
Vc=-（Rtmx*Vtmx+Rtmy*Vtmy）/Rtm;?%彈目相對速度

q=1;
Am=0;

dSightAngle=0;
%?b1（11）=Rtm;
while（1）
????rmx2（1q）=Rmx;
????rmy2（1q）=Rmy;
????rtx2（1q）=Rtx;
????rty2（1q）=Rty;
????
????a2（1q）=Am;
????time2（1q）=Time;
????
????dq1（1q）=SightAngle;
????dq2（1q）=dSightAngle;
????b1（1q）=Rtm;
????t1（1q）=ThetaT;
????vm1（1q）=Vm;
????if（abs（Rtm）<1）?%擊中目標
????????break;
????end
???if（abs（Rtm）<500）
???????TimeStep=0.00005;
???end
???
???dSightAngle=（Rtmx*Vtmy-Rtmy*Vtmx）/（Rtm^2）;??%視線角速率
???kd=3;
???
???%----------導彈加速度（導彈加速度矢量垂直于視線角速度方向）-------
???Am=Vc*dSightAngle*kd;?%比例導引律
???if?Am>AmMay?%限制機動能力
???????Am=AmMay;
???end
???if?Am<-AmMay
???????Am=-AmMay;
???end
????Amx=-Am*sin（SightAngle）;?%沿x軸的加速度分量
????Amy=Am*cos（SightAngle）;??%沿Y軸的加速度分量
????%---------狀態刷新-----------------
????Time=Time+TimeStep;
?
????Rmx=Rmx+TimeStep*Vmx;