具有角度和时间约束的导弹最优全弹道设计（源代码）
%具有角度和时间约束的导弹最优全弹道设计
%根据论文，初制导功能在于控制导弹快速爬升
%指令高度：Hc=1000+35*t，以35m/s快速爬升
%指令偏航角为-45deg
clear
clc
%---------------------导弹参数设置-----------------------
V=260; % 单位：m/s，导弹飞行速度
theta=45*pi/180;%单位：rad，初始弹道倾角
fea=-45*pi/180;%单位：rad，初始弹道偏角
T_1=14.2;%单位：s，初制导时间
H=1000;%单位：m，飞行高度
g=9.6;%单位：m/s^2，重力加速度
X=0; %单位：m，拦截弹位置坐标，X轴
Y=1000; %单位：m，拦截弹位置坐标，Y轴
Z=0; %单位：m，拦截弹位置坐标，Z轴
%------------------变结构控制律参数设置-----------------------
c1=30; %纵向通道
epu1=.1;%纵向通道
k1=.1;%纵向通道
c2=30;%偏航通道
epu2=.0005;%偏航通道
k2=.5;%偏航通道
dH=0;
dfea=0;
t=0; %单位：s，仿真时刻
dt=0.001;%单位：s，仿真时间步长
n=1;%计数用
for i=1:14200 %循环14200次，对应初制导时间14.2s
%-------------------------纵向通道------------------------------
Hc=1000+35*t; %高度指令
dHc=35;%高度指令的导数
ddHc=0;%高度指令的二阶导数
e1=Hc-H; %高度误差
de1=dHc-dH; %高度误差的导数
S1=c1*e1+de1; %切换函数（论文中式（4））
dS1=-epu1*sign(S1)-k1*S1;%指数趋近律（论文中式（5））
ny=1/g*(c1*(dHc-dH)+ddHc+(epu1+10)*sign(S1)+k1*S1); %控制输入（论文中式（6））
dH=V*sin(theta); %纵向通道：高度变化函数（论文中式（1））
dtheta=g/V*(ny-cos(theta)); %纵向通道：弹道倾角变化函数（论文中式（1））
H=H+dH*dt;
theta=theta+dtheta*dt;
%-------------------------偏航通道------------------------------
feac=-45*pi/180; %弹道偏角指令
dfeac=0;%弹道偏角指令的导数
e2=feac-fea; %弹道偏角误差
de2=dfeac-dfea; %弹道偏角误差的导数
S2=c2*e2+de2; %切换函数
dS2=-epu2*sign(S2)-k2*S2;%指数趋近律
nz=-V/g*(c2*(feac-fea)+dfeac+(epu2)*sign(S2)+k2*S2); %控制输入（论文中式（8））
dfea=-g/(V*cos(theta))*nz;%偏航通道：弹道偏角变化函数（论文中式（7））
fea=fea+dfea*dt;
%----------------------------计算坐标----------------------------
X=X+V*cos(theta)*cos(fea)*dt;
Y=Y+V*sin(theta)*dt;
Z=Z-V*cos(theta)*sin(fea)*dt;
%-------------------------保存结果------------------------------
H_store(:,n)=[H;Hc]; %第一行为实际飞行高度，第二行为指令高度
theta_store(n)=theta; %保存弹道倾角
fea_store(:,n)=[fea;feac]; %第一行为实际弹道偏角，第二行为指令弹道偏角
ny_store(n)=ny; %保存纵向过载
nz_store(n)=nz; %保存偏航过载
P_store(:,n)=[X;Y;Z];
n=n+1; %每循环一次，计数加一
t=t+dt; %往前推进一个仿真步长
end
figure(1)
plot((1:n-1)*dt,H_store(1,:),(1:500:n-1)*dt,H_store(2,1:500:end),'+')
xlabel('time/s')
ylabel('Altitude/m')
legend('实际飞行高度','指令高度')
title('初制导：高度变化情况')
figure(2)
plot((1:n-1)*dt,theta_store*180/pi)
xlabel('time/s')
ylabel('弹道倾角/deg')
title('初制导：弹道倾角变化情况')
figure(3)
plot((1:n-1)*dt,ny_store)
xlabel('time/s')
ylabel('纵向过载ny')
title('初制导：纵向过载变化情况')
figure(4)
plot((1:n-1)*dt,fea_store(1,:)*180/pi,(1:500:n-1)*dt,fea_store(2,1:500:end)*180/pi,'+')
xlabel('time/s')
ylabel('弹道偏角/deg')
legend('实际弹道偏角','指令弹道偏角')
title('初制导：弹道偏角变化情况')
figure(5)
plot((1:n-1)*dt,nz_store)
xlabel('time/s')
ylabel('偏航过载nz')
title('初制导：偏航过载变化情况')
figure(6)%注意matlab三维坐标画图X、Y、Z轴方向与导弹飞行力学坐标系不同
plot3(P_store(1,:),P_store(3,:),P_store(2,:))
xlabel('X/m')
ylabel('-Z/m')
zlabel('Y/m')
title('拦截弹位置坐标')
%具有角度和时间约束的导弹最优全弹道设计
%根据论文，中制导功能在于控制偏航角，纵向通道论文中没有明确说明
%我们认为在中制导段，纵向高度保持在初制导结束时的状态，此处仍采用算法一来控制纵向通道
%采用算法二控制偏航通道
clear
clc
%---------------------导弹参数设置-----------------------
V=260; % 单位：m/s，导弹飞行速度
theta=10*pi/180;%单位：rad，初始弹道倾角
fea=-45*pi/180;%单位：rad，初始弹道偏角
T_2=85;%单位：s，中制导时间
H=1500;%单位：m，飞行高度
g=9.6;%单位：m/s^2，重力加速度
X=0; %单位：m，拦截弹位置坐标，X轴
Y=H; %单位：m，拦截弹位置坐标，Y轴
Z=0; %单位：m，拦截弹位置坐标，Z轴
%------------------纵向通道变结构控制律参数设置-----------------------
c1=5; %纵向通道
epu1=.1;%纵向通道
k1=.1;%纵向通道
%----------------------最优制导边界条件--------------------------
fea_A=fea; %中制导初始弹道偏角
fea_B=120*pi/180; %中制导结束时期望的弹道偏角
%x_B=18000*cos(pi/3)/V;
x_B=18000/V;
x_A=0;
%z_B=-22100*sin(pi/3)/V;
z_B=-12822/V;
z_A=0;
t_B=85;
t_A=0;
%------------------------一般初始条件---------------------------
dH=0;
dfea=0;
t=0; %单位：s，仿真时刻
dt=0.01;%单位：s，仿真时间步长
n=1;%计数用
for i=1:8500 %循环8500次，对应中制导时间85s
%-------------------------纵向通道------------------------------
Hc=1500; %高度指令
dHc=0;%高度指令的导数
ddHc=0;%高度指令的二阶导数
e1=Hc-H; %高度误差
de1=dHc-dH; %高度误差的导数
S1=c1*e1+de1; %切换函数（论文中式（4））
dS1=-epu1*sign(S1)-k1*S1;%指数趋近律（论文中式（5））
ny=1/g*(c1*(dHc-dH)+ddHc+(epu1+10)*sign(S1)+k1*S1); %控制输入（论文中式（6））
dH=V*sin(theta); %纵向通道：高度变化函数（论文中式（1））
dtheta=g/V*(ny-cos(theta)); %纵向通道：弹道倾角变化函数（论文中式（1））
H=H+dH*dt;
theta=theta+dtheta*dt;
%-------------------------偏航通道------------------------------
A=[(fea_B-fea_A)/2+(sin(2*fea_B)-sin(2*fea_A))/4 -1/2*((sin(fea_B))^2-(sin(fea_A))^2) sin(fea_B)-sin(fea_A);
-1/2*((sin(fea_B))^2-(sin(fea_A))^2) (fea_B-fea_A)/2-(sin(2*fea_B)-sin(2*fea_A))/4 cos(fea_B)-cos(fea_A);
sin(fea_B)-sin(fea_A) cos(fea_B)-cos(fea_A) fea_B-fea_A];
ANS=inv(A)*[-(x_B-x_A); -(z_B-z_A); -(t_B-t_A)];
lam1=ANS(1);
lam2=ANS(2);
lam3=ANS(3);
nz=-V/(lam1*cos(fea)-lam2*sin(fea)+lam3)/g; %控制输入（论文中式（17），（17）式算出来的是加速度，需要除以g得到过载）
dfea=-g/(V*cos(theta))*nz;%偏航通道：弹道偏角变化函数（论文中式（7））
fea=fea+dfea*dt;
%----------------------------计算坐标----------------------------
X=X+V*cos(theta)*cos(fea)*dt;
Y=Y+V*sin(theta)*dt;
Z=Z-V*cos(theta)*sin(fea)*dt;
%-------------------------保存结果------------------------------
H_store(:,n)=[H;Hc]; %第一行为实际飞行高度，第二行为指令高度
theta_store(n)=theta; %保存弹道倾角
fea_store(n)=fea; %第一行为实际弹道偏角，第二行为指令弹道偏角
ny_store(n)=ny; %保存纵向过载
nz_store(n)=nz; %保存偏航过载
P_store(:,n)=[X;Y;Z];
n=n+1; %每循环一次，计数加一
t=t+dt; %往前推进一个仿真步长
end
figure(1)
plot((1:n-1)*dt,H_store(1,:),(1:500:n-1)*dt,H_store(2,1:500:end),'+')
xlabel('time/s')
ylabel('Altitude/m')
legend('实际飞行高度','指令高度')
title('中制导：高度变化情况')
figure(2)
plot((1:n-1)*dt,theta_store*180/pi)
xlabel('time/s')
ylabel('弹道倾角/deg')
title('中制导：弹道倾角变化情况')
figure(3)
plot((1:n-1)*dt,ny_store)
xlabel('time/s')
ylabel('纵向过载ny')
title('中制导：纵向过载变化情况')
figure(4)
plot((1:n-1)*dt,fea_store*180/pi)
xlabel('time/s')
ylabel('弹道偏角/deg')
title('中制导：弹道偏角变化情况')
figure(5)
plot((1:n-1)*dt,nz_store)
xlabel('time/s')
ylabel('偏航过载nz')
title('中制导：偏航过载变化情况')
figure(6)%注意matlab三维坐标画图X、Y、Z轴方向与导弹飞行力学坐标系不同
plot3(P_store(1,:),P_store(3,:),P_store(2,:))
xlabel('X/m')
ylabel('-Z/m')
zlabel('Y/m')
title('拦截弹位置坐标')
%全弹道仿真
clear
clc
%---------------------拦截弹参数设置-----------------------
V_m=260; % 单位：m/s，导弹飞行速度
theta_m=45*pi/180;%单位：rad，初始弹道倾角
fea_m=10*pi/180;%单位：rad，初始弹道偏角
H=1000;%单位：m，飞行高度
g=9.6;%单位：m/s^2，重力加速度
X_m=0; %单位：m，拦截弹位置坐标，X轴
Y_m=1000; %单位：m，拦截弹位置坐标，Y轴
Z_m=0; %单位：m，拦截弹位置坐标，Z轴
%------------------算法一：变结构控制律参数设置-----------------------
c1=30; %纵向通道
epu1=.1;%纵向通道
k1=.1;%纵向通道
c2=30;%偏航通道
epu2=.0005;%偏航通道
k2=.5;%偏航通道
dH=0;
dfea_m=0;
t=0; %单位：s，仿真时刻
dt=0.01;%单位：s，仿真时间步长
n=1;%计数用
%===========================初制导阶段============================
for i=1:1420 %循环1420次，对应初制导时间14.2s
%-------------------------纵向通道------------------------------
Hc=1000+35*t; %高度指令
dHc=35;%高度指令的导数
ddHc=0;%高度指令的二阶导数
e1=Hc-H; %高度误差
de1=dHc-dH; %高度误差的导数
S1=c1*e1+de1; %切换函数（论文中式（4））
dS1=-epu1*sign(S1)-k1*S1;%指数趋近律（论文中式（5））
ny=1/g*(c1*(dHc-dH)+ddHc+(epu1+10)*sign(S1)+k1*S1); %控制输入（论文中式（6））
dH=V_m*sin(theta_m); %纵向通道：高度变化函数（论文中式（1））
dtheta_m=g/V_m*(ny-cos(theta_m)); %纵向通道：弹道倾角变化函数（论文中式（1））
H=H+dH*dt;
theta_m=theta_m+dtheta_m*dt;
%-------------------------偏航通道------------------------------
feac=-45*pi/180; %弹道偏角指令
dfeac=0;%弹道偏角指令的导数
e2=feac-fea_m; %弹道偏角误差
de2=dfeac-dfea_m; %弹道偏角误差的导数
S2=c2*e2+de2; %切换函数
dS2=-epu2*sign(S2)-k2*S2;%指数趋近律
nz=-V_m/g*(c2*(feac-fea_m)+dfeac+(epu2)*sign(S2)+k2*S2); %控制输入（论文中式（8））
dfea_m=-g/(V_m*cos(theta_m))*nz;%偏航通道：弹道偏角变化函数（论文中式（7））
fea_m=fea_m+dfea_m*dt;
%----------------------------计算坐标----------------------------
X_m=X_m+V_m*cos(theta_m)*cos(fea_m)*dt;
Y_m=Y_m+V_m*sin(theta_m)*dt;
Z_m=Z_m-V_m*cos(theta_m)*sin(fea_m)*dt;
%-------------------------保存结果------------------------------
H_store(:,n)=[H;Hc]; %第一行为实际飞行高度，第二行为指令高度
theta_m_store(n)=theta_m; %保存弹道倾角
fea_m_store(n)=fea_m; %第一行为实际弹道偏角，第二行为指令弹道偏角
ny_store(n)=ny; %保存纵向过载
nz_store(n)=nz; %保存偏航过载
P_m_store(:,n)=[X_m;Y_m;Z_m];
n=n+1; %每循环一次，计数加一
t=t+dt; %往前推进一个仿真步长
end
% X_m
% Y_m
% Z_m
% theta_m
% fea_m
% pause
%===========================中制导阶段============================
%----------------------最优制导边界条件--------------------------
fea_A=fea_m; %中制导初始弹道偏角
fea_B=120*pi/180; %中制导结束时期望的弹道偏角
x_B=(18000+X_m)/V_m;
x_A=X_m/V_m;
z_B=(Z_m-12423)/V_m;
z_A=Z_m/V_m;
t_B=85;
t_A=0;
for i=1:8500%循环8500次，对应中制导时间85s
%-------------------------纵向通道------------------------------
Hc=1497; %高度指令
dHc=0;%高度指令的导数
ddHc=0;%高度指令的二阶导数
e1=Hc-H; %高度误差
de1=dHc-dH; %高度误差的导数
S1=c1*e1+de1; %切换函数（论文中式（4））
dS1=-epu1*sign(S1)-k1*S1;%指数趋近律（论文中式（5））
ny=1/g*(c1*(dHc-dH)+ddHc+(epu1+10)*sign(S1)+k1*S1); %控制输入（论文中式（6））
dH=V_m*sin(theta_m); %纵向通道：高度变化函数（论文中式（1））
dtheta_m=g/V_m*(ny-cos(theta_m)); %纵向通道：弹道倾角变化函数（论文中式（1））
H=H+dH*dt;
theta_m=theta_m+dtheta_m*dt;
%-------------------------偏航通道------------------------------
A=[(fea_B-fea_A)/2+(sin(2*fea_B)-sin(2*fea_A))/4 -1/2*((sin(fea_B))^2-(sin(fea_A))^2) sin(fea_B)-sin(fea_A);
-1/2*((sin(fea_B))^2-(sin(fea_A))^2) (fea_B-fea_A)/2-(sin(2*fea_B)-sin(2*fea_A))/4 cos(fea_B)-cos(fea_A);
sin(fea_B)-sin(fea_A) cos(fea_B)-cos(fea_A) fea_B-fea_A];
ANS=inv(A)*[-(x_B-x_A); -(z_B-z_A); -(t_B-t_A)];
lam1=ANS(1);
lam2=ANS(2);
lam3=ANS(3);
nz=-V_m/(lam1*cos(fea_m)-lam2*sin(fea_m)+lam3)/g; %控制输入（论文中式（17）,（17）式除以g得到过载）
dfea_m=-g/(V_m*cos(theta_m))*nz;%偏航通道：弹道偏角变化函数（论文中式（7））
fea_m=fea_m+dfea_m*dt;
%----------------------------计算坐标----------------------------
X_m=X_m+V_m*cos(theta_m)*cos(fea_m)*dt;
Y_m=Y_m+V_m*sin(theta_m)*dt;
Z_m=Z_m-V_m*cos(theta_m)*sin(fea_m)*dt;
%-------------------------保存结果------------------------------
H_store(:,n)=[H;Hc]; %第一行为实际飞行高度，第二行为指令高度
theta_m_store(n)=theta_m; %保存弹道倾角
fea_m_store(n)=fea_m; %第一行为实际弹道偏角，第二行为指令弹道偏角
ny_store(n)=ny; %保存纵向过载
nz_store(n)=nz; %保存偏航过载
P_m_store(:,n)=[X_m;Y_m;Z_m];
n=n+1; %每循环一次，计数加一
t=t+dt; %往前推进一个仿真步长
end
X_m
Y_m
Z_m
theta_m
fea_m
%===========================末制导阶段============================
%---------------------目标弹参数设置-----------------------
V_t=150; %单位：m/s，目标弹飞行速度
theta_t=10*pi/180;%单位：rad，初始弹道偏角
fea_t=80*pi/180;%单位：rad，初始弹道偏角
X_t=-14000; %单位：m，目标弹位置坐标，X轴
Y_t=2500; %单位：m，目标弹位置坐标，Y轴
Z_t=-8500; %单位：m，目标弹位置坐标，Z轴
%------------------算法三：比例导引律参数设置-----------------------
k11=10; %纵向通道导引系数
k12=10;%偏航通道导引系数
R=sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2) % 计算弹目相对距离
theta_L=atan((Y_t-Y_m)/sqrt((X_t-X_m)^2+(Z_t-Z_m)^2)) %视线倾角
fea_L=atan((Z_t-Z_m)/(X_t-X_m)) %视线偏角
pause
n2=1;
while(R>1) %当弹目相对距离小于阈值时，仿真停止
%-------------------------纵向通道------------------------------
% dR=V_t*cos(theta_L)*cos(fea_L-fea_t)-(cos(theta_m)*cos(theta_L)*cos(fea_L-fea_m)+sin(theta_m)*sin(theta_L))*V_m;
% dtheta_L=-1/R*V_t*sin(theta_L)*cos(fea_L-fea_t)-1/R*(sin(theta_m)*cos(theta_L)-sin(theta_L)*cos(theta_m)*cos(fea_L-fea_m))*V_m;
dX_m=V_m*cos(theta_m)*cos(fea_m);
dY_m=V_m*sin(theta_m);
dZ_m=-V_m*cos(theta_m)*sin(fea_m);
dX_t=V_t*cos(theta_t)*cos(fea_t);
dY_t=V_t*sin(theta_t);
dZ_t=-V_t*cos(theta_t)*sin(fea_t);
dR=((X_m-X_t)*(dX_m-dX_t)+(Y_m-Y_t)*(dY_m-dY_t)+(Z_m-Z_t)*(dZ_m-dZ_t))/sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);
dtheta_L=((dY_t-dY_m)*sqrt((X_t-X_m)^2+(Z_t-Z_m)^2)-(Y_t-Y_m)*((X_t-X_m)*(dX_t-dX_m)+(Z_t-Z_m)*(dZ_t-dZ_m))/sqrt((X_t-X_m)^2+(Z_t-Z_m)^2))/((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);
ny=k11*abs(dR)*dtheta_L/g; %控制输入（论文中式（18）第一式）
dtheta_m=g/V_m*(ny-cos(theta_m)); %纵向通道：弹道倾角变化函数（论文中式（1））
theta_m=theta_m+dtheta_m*dt;
%-------------------------偏航通道------------------------------
% dfea_L=1/(R*cos(theta_L))*V_t*sin(fea_L-fea_t)-1/(R*cos(theta_L))*V_m*cos(theta_m)*sin(fea_L-fea_m);
dfea_L=((dZ_t-dZ_m)*(X_t-X_m)-(Z_t-Z_m)*(dX_t-dX_m))/((X_t-X_m)^2+(Z_t-Z_m)^2);
nz=k12*abs(dR)*dfea_L/g; %控制输入（论文中式（18）第二式）
dfea_m=-g/(V_m*cos(theta_m))*nz;%偏航通道：弹道偏角变化函数（论文中式（7））
fea_m=fea_m+dfea_m*dt;
%----------------------------计算拦截弹坐标----------------------------
X_m=X_m+V_m*cos(theta_m)*cos(fea_m)*dt;
Y_m=Y_m+V_m*sin(theta_m)*dt;
Z_m=Z_m-V_m*cos(theta_m)*sin(fea_m)*dt;
%----------------------------计算目标弹坐标----------------------------
X_t=X_t+V_t*cos(theta_t)*cos(fea_t)*dt;
Y_t=Y_t+V_t*sin(theta_t)*dt;
Z_t=Z_t-V_t*cos(theta_t)*sin(fea_t)*dt;
%计算相对运动学参数;
R=sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);
theta_L=atan((Y_t-Y_m)/sqrt((X_t-X_m)^2+(Z_t-Z_m)^2));
fea_L=atan((Z_t-Z_m)/(X_t-X_m));
%-------------------------保存结果------------------------------
theta_m_store(n)=theta_m; %保存弹道倾角
fea_m_store(n)=fea_m; %第一行为实际弹道偏角，第二行为指令弹道偏角
ny_store(n)=ny; %保存纵向过载
nz_store(n)=nz; %保存偏航过载
P_m_store(:,n)=[X_m;Y_m;Z_m]; %保存拦截弹坐标
P_t_store(:,n2)=[X_t;Y_t;Z_t]; %保存目标弹坐标
n=n+1; %每循环一次，计数加一
n2=n2+1;
t=t+dt; %往前推进一个仿真步长
end
disp('脱靶量为（m）：')
R
figure(1)
plot((1:n-1)*dt,theta_m_store*180/pi)
xlabel('time/s')
ylabel('弹道倾角/deg')
title('弹道倾角变化情况')
figure(2)
plot((1:n-1)*dt,ny_store)
xlabel('time/s')
ylabel('纵向过载ny')
title('纵向过载变化情况')
figure(3)
plot((1:n-1)*dt,fea_m_store(1,:)*180/pi)
xlabel('time/s')
ylabel('弹道偏角/deg')
title('弹道偏角变化情况')
figure(4)
plot((1:n-1)*dt,nz_store)
xlabel('time/s')
ylabel('偏航过载nz')
title('偏航过载变化情况')
figure(5)%注意matlab三维坐标画图X、Y、Z轴方向与导弹飞行力学坐标系不同
plot3(P_m_store(1,:),P_m_store(3,:),P_m_store(2,:),P_t_store(1,:),P_t_store(3,:),P_t_store(2,:))
legend('拦截弹运动轨迹','目标弹运动轨迹')
xlabel('X/m')
ylabel('-Z/m')
zlabel('Y/m')%具有角度和时间约束的导弹最优全弹道设计
%算法三：比例导引末制导
%说明：在仿真中，下标"_m"表示拦截弹参数，下标"_t"表示目标弹参数
clear
clc
%---------------------拦截弹参数设置-----------------------
V_m=260; % 单位：m/s，导弹飞行速度
theta_m=45*pi/180;%单位：rad，初始弹道倾角
fea_m=10*pi/180;%单位：rad，初始弹道偏角
H=1000;%单位：m，飞行高度
g=9.6;%单位：m/s^2，重力加速度
X_m=0; %单位：m，拦截弹位置坐标，X轴
Y_m=1000; %单位：m，拦截弹位置坐标，Y轴
Z_m=0; %单位：m，拦截弹位置坐标，Z轴
%---------------------目标弹参数设置-----------------------
V_t=150; %单位：m/s，目标弹飞行速度
theta_t=60*pi/180;%单位：rad，初始弹道偏角
fea_t=0*pi/180;%单位：rad，初始弹道偏角
X_t=1000; %单位：m，目标弹位置坐标，X轴
Y_t=2000; %单位：m，目标弹位置坐标，Y轴
Z_t=10*pi/180; %单位：m，目标弹位置坐标，Z轴
%------------------比例导引律参数设置-----------------------
k1=5; %纵向通道导引系数
k2=5;%偏航通道导引系数
t=0; %单位：s，仿真时刻
dt=0.001;%单位：s，仿真时间步长
n=1;%计数用
R=sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2); % 计算弹目相对距离
theta_L=atan((Y_t-Y_m)/sqrt((X_t-X_m)^2)+(Z_t-Z_m)^2);
fea_L=atan((Z_t-Z_m)/(X_t-X_m));
while(R>1) %当弹目相对距离小于阈值时，仿真停止
%-------------------------纵向通道------------------------------
% dR=V_t*cos(theta_L)*cos(fea_L-fea_t)-(cos(theta_m)*cos(theta_L)*cos(fea_L-fea_m)+sin(theta_m)*sin(theta_L))*V_m;
% dtheta_L=-1/R*V_t*sin(theta_L)*cos(fea_L-fea_t)-1/R*(sin(theta_m)*cos(theta_L)-sin(theta_L)*cos(theta_m)*cos(fea_L-fea_m))*V_m;
dX_m=V_m*cos(theta_m)*cos(fea_m);
dY_m=V_m*sin(theta_m);
dZ_m=-V_m*cos(theta_m)*sin(fea_m);
dX_t=V_t*cos(theta_t)*cos(fea_t);
dY_t=V_t*sin(theta_t);
dZ_t=-V_t*cos(theta_t)*sin(fea_t);
dR=((X_m-X_t)*(dX_m-dX_t)+(Y_m-Y_t)*(dY_m-dY_t)+(Z_m-Z_t)*(dZ_m-dZ_t))/sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);
dtheta_L=((dY_t-dY_m)*sqrt((X_t-X_m)^2+(Z_t-Z_m)^2)-(Y_t-Y_m)*((X_t-X_m)*(dX_t-dX_m)+(Z_t-Z_m)*(dZ_t-dZ_m))/sqrt((X_t-X_m)^2+(Z_t-Z_m)^2))/((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);
ny=k1*abs(dR)*dtheta_L/g; %控制输入（论文中式（18）第一式）
dtheta_m=g/V_m*(ny-cos(theta_m)); %纵向通道：弹道倾角变化函数（论文中式（1））
theta_m=theta_m+dtheta_m*dt;
%-------------------------偏航通道------------------------------
% dfea_L=1/(R*cos(theta_L))*V_t*sin(fea_L-fea_t)-1/(R*cos(theta_L))*V_m*cos(theta_m)*sin(fea_L-fea_m);
dfea_L=((dZ_t-dZ_m)*(X_t-X_m)-(Z_t-Z_m)*(dX_t-dX_m))/((X_t-X_m)^2+(Z_t-Z_m)^2);
nz=k2*abs(dR)*dfea_L/g; %控制输入（论文中式（18）第二式）
dfea_m=-g/(V_m*cos(theta_m))*nz;%偏航通道：弹道偏角变化函数（论文中式（7））
fea_m=fea_m+dfea_m*dt;
%----------------------------计算拦截弹坐标----------------------------
X_m=X_m+V_m*cos(theta_m)*cos(fea_m)*dt;
Y_m=Y_m+V_m*sin(theta_m)*dt;
Z_m=Z_m-V_m*cos(theta_m)*sin(fea_m)*dt;
%----------------------------计算目标弹坐标----------------------------
X_t=X_t+V_t*cos(theta_t)*cos(fea_t)*dt;
Y_t=Y_t+V_t*sin(theta_t)*dt;
Z_t=Z_t-V_t*cos(theta_t)*sin(fea_t)*dt;
%计算相对运动学参数
R=sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);
theta_L=atan((Y_t-Y_m)/sqrt((X_t-X_m)^2)+(Z_t-Z_m)^2);
fea_L=atan((Z_t-Z_m)/(X_t-X_m));
%-------------------------保存结果------------------------------
theta_m_store(n)=theta_m; %保存弹道倾角
fea_m_store(n)=fea_m; %第一行为实际弹道偏角，第二行为指令弹道偏角
ny_store(n)=ny; %保存纵向过载
nz_store(n)=nz; %保存偏航过载
P_m_store(:,n)=[X_m;Y_m;Z_m]; %保存拦截弹坐标
P_t_store(:,n)=[X_t;Y_t;Z_t]; %保存目标弹坐标
n=n+1; %每循环一次，计数加一
t=t+dt; %往前推进一个仿真步长
end
disp('脱靶量为（m）：')
R
figure(1)
plot((1:n-1)*dt,theta_m_store*180/pi)
xlabel('time/s')
ylabel('弹道倾角/deg')
title('末制导：弹道倾角变化情况')
figure(2)
plot((1:n-1)*dt,ny_store)
xlabel('time/s')
ylabel('纵向过载ny')
title('末制导：纵向过载变化情况')
figure(3)
plot((1:n-1)*dt,fea_m_store(1,:)*180/pi)
xlabel('time/s')
ylabel('弹道偏角/deg')
title('末制导：弹道偏角变化情况')
figure(4)
plot((1:n-1)*dt,nz_store)
xlabel('time/s')
ylabel('偏航过载nz')
title('末制导：偏航过载变化情况')
figure(5)%注意matlab三维坐标画图X、Y、Z轴方向与导弹飞行力学坐标系不同
plot3(P_m_store(1,:),P_m_store(3,:),P_m_store(2,:),P_t_store(1,:),P_t_store(3,:),P_t_store(2,:))
legend('拦截弹运动轨迹','目标弹运动轨迹')
xlabel('X/m')
ylabel('-Z/m')
zlabel('Y/m')