%tbm被动段弹道仿真
%红色段导弹为再入段弹道，蓝色为自由飞行段,
%figue(1)为弹道曲线,figue(2)为速度变化,
clear;
clc;
RHO=1.225;
SONIC=343.13;
dt=0.1;
v0=8*SONIC;
h0=40000; %关机点高度
xt0=810000;
yt0=h0;
h1=80000; %再入段高度
g=9.81;
ae=pi/180;
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;
thetatz=atan((v0*sin(thetat0)-g*T)/(v0*cos(thetat0)));
figure(1);plot(x1,y1),hold on, %自由段坐标变化
figure(2);plot(T,sqrt(vx0.^2+(vy0-g*T).^2)),hold on, %自由段速度变化
%figure(3);plot(T,thetatz),hold on,
R=6371000;
g=9.81;
m=100;
CX=5;
d=0.1698;
i=0.5;
ae=pi/180;
thetat=abs(thetat1); %初始弹道倾角
xt=xt1;
yt=h1;
vt=v1; %初速
ptr(:,1)=[xt;yt];
ptvr(:,1)=[t;vt];
for k=2:4000
F=4.75*0.0001*vt^2*CX;
if yt>=9300
H=exp(-0.0001*yt);
end
if yt<9300
H=(1-2.1905*10^(-5*yt))^4.4;
end
XF=i*d^2*1000*H*F./g;
dvt=g*sin(thetat)-XF/m;
dthetat=-g*cos(thetat)/vt+vt*cos(thetat)/(R+yt);
dyt=-vt*sin(thetat);
dxt=R*vt*cos(thetat)/(R+yt);
vt=vt+dt*dvt;
thetat=thetat-dt*dthetat;
yt=yt+dt*dyt;
xt=xt-dt*dxt;
ptr(:,k)=[xt;yt];
ptvr(:,k)=[t+k*dt;vt];
if yt<=1000 break;end;
%plot(xt,yt),hold on,
%figure(2);plot(t+k*dt,vt,'r'),hold on, %再入段速度变化 打点方式
%figure(3);plot(t+k*dt,thetat*57.3+90,'r'),hold on,
end
figure(1);plot(ptr(1,:),ptr(2,:),'r');
figure(2);plot(ptvr(1,:),ptvr(2,:),'r'); %再入段速度变化 ptvr方式
