帶落角限制的變結構末制導律（線性） 源代碼

參考文獻：周荻那本變結構導引律的設計

clear

%===================================================================================
%--------------------------------情景說明------------------------------------------
%目标正弦機動,At=Asin(*),速度大小不變
%飛彈速度大小為常量
%==========================================================================
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;

Q=atan((Yt-Ym)/(Xt-Xm));
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=40;
E2=0.01;
c=2;
Qd=-pi/2;
At=20;
while(DotR<=0)
        DotQ1(n)=DotQ;
        Q1(n)=Q;
        
        %if(abs(t-3)<=1e-6 || abs(t-6)<=1e-6 || abs(t-9)<=1e-6 || abs(t-12)<=1e-6)
         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;
      
        R=sqrt((Yt-Ym)^2+(Xt-Xm)^2);
  
        Am=((k+1)*abs(DotR)+c*Vm)*DotQ+(k*abs(DotR)/R*c*Vm)*(Q-Qd)+E1*(DotQ+c*Vm/R*(Q-Qd))/(abs(DotQ+c*Vm/R*(Q-Qd))+E2);        
        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;
        
        Q=atan((Yt-Ym)/(Xt-Xm));
        angleV(n)=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('AngleRate/Deg')
grid on

figure(4)
plot((1:(n-20))*Dt,Q1(1:(n-20))*180/pi)
xlabel('Time/s')
ylabel('Angle/Deg')
grid on

%disp('運作PlotResult.m')