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

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

%===================================================================================%--------------------------------情景说明------------------------------------------%目标正弦机动,At=Asin(*),速度大小不变%导弹速度大小为常量%==========================================================================close allclearclcdisp('----------------------程序开始运行----------------------')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;endfigure(1)plot(Xt1,Yt1,Xm1,Ym1)title('Trajectory of Missile and Target')xlabel('Downrange/m')ylabel('Attitude/m')grid onfigure(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 onfigure(3)plot((1:(n-20))*Dt,DotQ1(1:(n-20))*180/pi)xlabel('Time/s')ylabel('AngelRate/Deg')grid onfigure(4)plot((1:(n-20))*Dt,Q1(1:(n-20))*180/pi)%disp('运行VSSStore.m....')