带落角约束的变结构末制导律（非线性）源代码

参考文献：周荻那本变结构制导律的设计

%===================================================================================
%--------------------------------情景说明------------------------------------------
%目标正弦机动,At=Asin(*),速度大小不变
%导弹速度大小为常量
%==========================================================================
close all
clear
clc
disp('----------------------程序开始运行----------------------')
Vt=500;
Xt=10000;
Yt=8000;
ThetaT=pi/9;
Xt1=[]; %存放中间数据，用来分析结果，如作图
Yt1=[];
At1=[];

Vm=2000;
Xm=0;
Ym=0;
ThetaM=2*pi/9;
Xm1=[]; %存放中间数据，用来分析结果，如作图
Ym1=[];
Am1=[];
AngleV=[];
DotQ1=[];
Q1=[];

n=1;
Xt1(n)=Xt;
Yt1(n)=Yt;

Xm1(n)=Xm;
Ym1(n)=Ym;


DotQ=((Vt*sin(ThetaT)-Vm*sin(ThetaM))*(Xt-Xm)-(Yt-Ym)*(Vt*cos(ThetaT)-Vm*cos(ThetaM)))/((Xt-Xm)^2+(Yt-Ym)^2);
DotR=((Xm-Xt)*(Vm*cos(ThetaM)-Vt*cos(ThetaT))+(Ym-Yt)*(Vm*sin(ThetaM)-Vt*sin(ThetaT)))/sqrt((Xm-Xt)^2+(Ym-Yt)^2);

t=0;
Dt=0.01;
k=10;
E1=10;
E2=0.1;
c=5;
Qd=-pi/2;
At=20;
while(DotR<=0)
        DotQ1(n)=DotQ;
        R=sqrt((Yt-Ym)^2+(Xt-Xm)^2);
       if(mod(t,3)<0.01)
        	At=-At;
    	end
	    Xt=Xt+Vt*cos(ThetaT)*Dt+1/2*At*sin(ThetaT)*Dt^2;
        Yt=Yt+Vt*sin(ThetaT)*Dt+1/2*At*cos(ThetaT)*Dt^2;
        ThetaT=ThetaT+At/Vt*Dt;
        At1(n)=At;
        
        Q=atan((Yt-Ym)/(Xt-Xm)); 
        Q1(n)=Q;      
        Am=(-Vt*(DotQ-At/Vt)*cos(Q-ThetaT)+Vm*DotQ*cos(Q-ThetaM)+k*abs(DotR)*DotQ+k*abs(DotR)/R*c*Vm*(Q-Qd)+c*Vm*DotQ+E1*(DotQ+c*Vm/R*(Q-Qd))/(abs(DotQ+c*Vm/R*(Q-Qd))+E2))/cos(Q-ThetaM);
        Xm=Xm+Vm*cos(ThetaM)*Dt+1/2*Am*sin(ThetaM)*Dt^2;
        Ym=Ym+Vm*sin(ThetaM)*Dt+1/2*Am*cos(ThetaM)*Dt^2;
        
        AngelV(n)=ThetaM-Q;
        
        ThetaM=ThetaM+Am/Vm*Dt;
        Am1(n)=Am;
        
        DotQ=((Vt*sin(ThetaT)-Vm*sin(ThetaM))*(Xt-Xm)-(Yt-Ym)*(Vt*cos(ThetaT)-Vm*cos(ThetaM)))/((Xt-Xm)^2+(Yt-Ym)^2);
        DotR=((Xm-Xt)*(Vm*cos(ThetaM)-Vt*cos(ThetaT))+(Ym-Yt)*(Vm*sin(ThetaM)-Vt*sin(ThetaT)))/sqrt((Xm-Xt)^2+(Ym-Yt)^2);
        
        n=n+1;
        Xt1(n)=Xt;
        Yt1(n)=Yt;

        Xm1(n)=Xm;
        Ym1(n)=Ym;
       
        t=t+Dt;
end

figure(1)
plot(Xt1,Yt1,Xm1,Ym1)
title('Trajectory of Missile and Target')
xlabel('Downrange/m')
ylabel('Attitude/m')
grid on


figure(2)
plot((1:(n-20))*Dt,Am1(1:(n-20))./9.8,(1:(n-20))*Dt,At1(1:(n-20))./9.8)
xlabel('Time/s')
ylabel('Acceleration/g')
grid on

figure(3)
plot((1:(n-20))*Dt,DotQ1(1:(n-20))*180/pi)
xlabel('Time/s')
ylabel('AngelRate/Deg')
grid on

figure(4)
plot((1:(n-20))*Dt,Q1(1:(n-20))*180/pi)

%disp('运行VSSStore.m....')