三维比例导引

%反战术导弹导弹比例导引律三维弹道仿真
%红色段为TBM自由飞行段弹道
%蓝色段为TBM再入段弹道
%黑色段为防空导弹弹道
clear;
ae=pi/180;
dt=0.01;
SONIC=343.13;
RHO=1.225;
vm=2000;

Mx0=0;Mz0=0;My0=0;
pmr（:1）=[Mx0;Mz0;My0];

v0=8*SONIC;
h0=40000;                         %关机点高度          
xt0=809000;           %809000比较合适的距离
yt0=h0;
h1=80000;                            %再入段高度
g=9.81;

thetat0=135*ae;                        %初始弹道倾角
vx0=v0*cos（thetat0）;
vy0=v0*sin（thetat0）;
t=（vy0+sqrt（vy0^2-2*g*（h1-h0）））/g;   %再入段时间
xt1=xt0+vx0*t;
yt1=h0+vy0*t-0.5*g*t^2;
vx1=vx0;
vy1=vy0-g*t;
v1=sqrt（vx1^2+vy1^2）;
thetat1=atan（（v0*sin（thetat0）-g*t）/（v0*cos（thetat0）））;%再入角

T=0:dt:t;
x1=xt0+vx0*T;
y1=h0+vy0*T-0.5*g*T.^2;
figure（3）;plot3（x10+0*Ty1‘r‘）hold on
%figure（2）;plot（Tsqrt（vx0.^2+（vy0-g*T）.^2））hold on            %自由段速度变化

R=6371000;
M=100;
CX=5;
d=0.1698;
i=0.5;

vt=v1;  
xt=xt1;
yt=h1;
thetat=abs（thetat1）;                        %初始弹道倾角
ptr（:1）=[xt;0;yt];
ptvr（:1）=[t;vt];

et=atan（ptr（31）/sqrt（ptr（11）^2+ptr（21）^2））;
bt=atan（ptr（21）/ptr（11））;
em=et;
bm=bt;

time=0;
for k=2:8000;
 r（k-1）=sqrt（（ptr（1k-1）-pmr（1k-1））^2+（ptr（2k-1）-pmr（2k-1））^2+（ptr（3k-1）-pmr（3k-1））^2）;
   i