变结构制导律源程序

参考文献：<一种变结构垂直俯冲攻击制导律>

clear
clc

%-----------导弹参数---------
V_m=300;
X_m=0;
Y_m=3000;
theta_m=0*pi/180;

%----------目标参数---------
V_t=0; %静止目标
X_t=3000;
Y_t=0;
theta_t=0;
dtheta_t=0;
n_t=0;
A_t=0;

%变结构制导律参数设置
k=1;
delta=0.01;
epu=.9;
lam=1.5;

R=sqrt((X_m-X_t)^2+(Y_m-Y_t)^2);
q=atan((Y_t-Y_m)/(X_t-X_m));
dR=((X_m-X_t)*(V_m*cos(theta_m)-V_t*cos(theta_t))+(Y_m-Y_t)*(V_m*sin(theta_m)-V_t*sin(theta_t)))/sqrt((X_m-X_t)^2+(Y_m-Y_t)^2);
dq=((X_t-X_m)*(V_t*sin(theta_t)-V_m*sin(theta_m))-(Y_t-Y_m)*(V_t*cos(theta_t)-V_m*cos(theta_m)))/((X_m-X_t)^2+(Y_m-Y_t)^2);

n_m=-q+theta_m;

n=1;
t=0;
dt=0.01;
hit_angle=-90*pi/180;  %期望命中角度

while (dR<0)
    x1=q-hit_angle;
    x2=dq;
     
    S=x2+lam*V_m*x1/R;
    
    fun=S/(abs(S)+delta);
   % Am=(V_m*(1-dtheta_t)-(k+2)*dR*x2)/cos(n_m)+(