弹道程序，基本上走通了

%% 程序说明
%2024年4月20日，
%isp各子级比冲；tcb推进剂填充比；mcd,尾端-发动机-级间段质量-末级-整流罩；
%m0火箭总质量；d2r角度转换；mi%i子级质量;mp各级发动机喷气量，kg/s；
%pa.tk,各级火箭飞行时间;mf发动机质量；pe各级发动机推力，N;
%t4四子级主动总飞行时间,%th四子级滑行时间；t41四子级第一次主动段时间，t43四子级第三次主动段时间;
%dad弹道数据；trs%主动段总时间;tfx最大飞行总时间;pt转弯段时间；
%%  参数
clc;clear all;clf;close all;global pa op dad;format long;
d2r=57.142857142857146;
%% 轨道参数
pa.sso=500000;
pa.vg=vh(pa.sso/1000);
pa.rg=pa.sso+6371e3;
dad=zeros(1,14);
%% 火箭参数
tcb=0.9;isp=[2400 2400 2400 3000];
mcd=[350 41111 400 11111 300 4111 160 1557 320];pa.m0=sum(mcd(1:8));
pa.mf=[mcd(2) mcd(4) mcd(6) 600/0.9];%各级发动机质量
m1=sum(mcd(1:3));m2=sum(mcd(4:5));m3=sum(mcd(6:7));m4=mcd(8);
pa.pe=1.01*[1200000 400000 150000 10000];
pa.mp=pa.pe./isp;pa.tk=tcb*pa.mf./pa.mp;
pa.trs=sum(pa.tk);N0=pa.pe(1)/pa.m0/9.8;
%四子级质量：1557kg,-整流罩320kg，推进剂580kg，结构干质量230kg，卫星407kg；
%% 转弯程序设置
%优化参数设置：
xam=20;xt=20;x1=24;x2=19;x3=-20;xt1=4.4;xh=200;x41=60;

op.t41=x41;op.th=xh;op.t43=pa.tk(4)-op.t41;
pa.tfx=pa.trs+op.th;
pa.tk=[pa.tk(1) pa.tk(2) pa.tk(3) op.t41 op.th];
op.t1=sqrt(40/(N0-1))*xt1/10;
op.t2=op.t1+xt;
op.am=pi/xam;
op.k1=x1/d2r/(-100);
op.k2=x2/d2r/(-100);
op.k3=x3/d2r/(-100);
op.a=0.1;
%%
%% 地球参数
pa.ae=6378.140e3;%赤道平均半径
pa.be=6356.755288e3;%地球极地平均直径
pa.J2=1.08263e-3;%带谐系数
pa.J=1.5*pa.J2;
pa.fM=398603e9;%m3/s2 引力系数
%发射点参数
pa.phi0=19.32/d2r;%发射点地心维度
pa.A0=-11.72/d2r; %射向 
pa.ae_=(pa.ae-pa.be)/pa.ae;%中间参数
pa.miu0=pa.ae_*sin(2*pa.phi0);
pa.we=7.292115e-5;% rad/s 地球自转角速度
%发射点参数，纬度、地心半径，发射点坐标
pa.B0=pa.phi0+pa.miu0;%O点地理维度
pa.R0=pa.ae*pa.be/sqrt(pa.ae*pa.ae*sin(pa.phi0)^2+pa.be^2*cos(pa.phi0)^2);
pa.Rox=-pa.R0*sin(pa.miu0)*cos(pa.A0);
pa.Roy=pa.R0*cos(pa.miu0);
pa.Roz=pa.R0*sin(pa.miu0)*sin(pa.A0);
%地球自转在发射坐标系下分量
pa.wex=pa.we*cos(pa.B0)*cos(pa.A0);
pa.wey=pa.we*sin(pa.B0);
pa.wez=-pa.we*cos(pa.B0)*sin(pa.A0);
pa.we_xyz=[pa.we*cos(pa.B0)*cos(pa.A0),pa.we*sin(pa.B0),-pa.we*cos(pa.B0)*sin(pa.A0)];
%% 牵连加速度
a11=pa.we_xyz(1)^2-pa.we^2;a12=pa.we_xyz(1)*pa.we_xyz(2);a21=a12;
a22=pa.we_xyz(2)^2-pa.we^2;a23=pa.we_xyz(2)*pa.we_xyz(3);a32=a23;
a33=pa.we_xyz(3)^2-pa.we^2;a13=pa.we_xyz(3)*pa.we_xyz(1);a31=a13;
pa.a=[[a11 a12 a13];[a21 a22 a23];[a31 a32 a33]];
%哥氏加速度
b11=0;b22=0;b33=0;b12=-2*pa.we_xyz(3);b21=-b12;b31=-2*pa.we_xyz(2);
b23=-2*pa.we_xyz(1);b13=-b31;b32=-b23;
pa.b=[[b11 b12 b13];[b21 b22 b23];[b31 b32 b33]];
%坐标系变换矩阵，发射系->PM
pa.G2PM=[[cos(pa.B0)*cos(pa.A0),sin(pa.B0),-cos(pa.B0)*sin(pa.A0)];[-sin(pa.B0)*cos(pa.A0),cos(pa.B0),sin(pa.B0)*sin(pa.A0)];[sin(pa.A0),0,cos(pa.A0)]];
%% 积分函数 
options=odeset('MaxStep',1,'RelTol',1e-1,'AbsTol',1e-1);
tspan1=(0:0.5:pa.tk(1));y1=[0 10 0 0 0 0 59.1e3];[t1,d1]=ode45(@dt,tspan1,y1,options);%一子级求解
y2=d1(end,:);y2(7)=y2(7)-mcd(2)*0.1-mcd(1)-mcd(3);%二子级分离后初始值
tspan2=(pa.tk(1):1:pa.tk(2)+pa.tk(1));[t2,d2]=ode45(@dt,tspan2,y2,options);%二子级求解
y3=d2(end,:);y3(7)=y3(7)-mcd(4)*0.1-mcd(5)-mcd(9);%三子级分离后初始值,抛罩
tspan3=((pa.tk(1)+pa.tk(2)):1:(pa.tk(2)+pa.tk(1)+pa.tk(3)));[t3,d3]=ode45(@dt,tspan3,y3,options);%3级求解
y4=d3(end,:);y4(7)=m4-mcd(9);%4子级分离后初始值
try
tspan4=((pa.tk(1)+pa.tk(2)+pa.tk(3)):1:pa.tfx);%4时间段
[t4,d4]=ode45(@dt,tspan4,y4,options);%4级求解
catch
disp('结束ode函数')
end
mg=dad(end,13)
hys=dad(end,11)
vys=dad(end,2)
ctays=dad(end,14)*d2r
% Huit(dad)

function dydt=dt(t,y7)
% 发射坐标系火箭弹道主动段计算程序
% 近似处理：地球为均质椭圆球；忽略二阶小量；
% 状态变量7个： Y= 【vx，vy,vz,，x，y,z,mass】   dydt=Y'
%%
global pa op dad;format long;dydt=zeros(7,1);
vx=y7(1);vy=y7(2);vz=y7(3);x=y7(4);y=y7(5);z=y7(6);mass=y7(7);
r_=norm([y7(4)+pa.Rox,y7(5)+pa.Roy,y7(6)+pa.Roz]);v_=sqrt(vx*vx+vy*vy+vz*vz);%地心矢径
sinphi=([y7(4),y7(5),y7(6)]+[pa.Rox,pa.Roy,pa.Roz])*(pa.we_xyz') /(r_*pa.we);
phiq=asin(sinphi);%当地维度
R=pa.ae*pa.be/norm([pa.ae*sin(phiq),pa.be*cos(phiq)]);%当地地球半径
h=r_-R;%当地高度
%攻角、速度倾角、偏角
sig=-asin(y7(3)/v_);psi=0;bet=0;
% phi 俯仰程序角度
if t<op.t1
    alp=0;
    theat=atan(vy/vx);
    phi=theat+alp;
elseif t<op.t2
    theat=atan(vy/vx);
    alp=-4*op.am*exp(-op.a*(t-op.t1))*(1-exp(-op.a*t+op.a*op.t1));
    phi=theat+alp;
    theat=atan(y7(2)/y7(1));
elseif t<sum(pa.tk(1:2))
    alp=0;theat=atan(vy/vx);
    phi=theat+alp;
    pa.phik2=phi;
elseif t<=sum(pa.tk(1:3))%三子级飞行程序角度
    theat=atan(vy/vx);
    phi=pa.phik2+op.k1*(t-sum(pa.tk(1:2)));
    alp=phi-theat;
    pa.phi3k=phi;
elseif t<=sum(pa.tk(1:4))%4子级飞行程序角度,第一主动段
    theat=atan(vy/vx);
    phi=pa.phi3k+op.k2*(t-sum(pa.tk(1:3)));
    alp=phi-theat;
    pa.phi4k1=phi;
elseif t<=sum(pa.tk(1:5))%4子级飞行程序角度,滑行段
%     phi=pa.phi4k1+op.k3*(t-(sum(pa.tk(1:4))));
    theat=atan(vy/vx);    
    phi=pa.phi4k1;
    alp=phi-theat;
    pa.phihk=phi;
else%瞄准，入轨，四级主动段
    theat=atan(vy/vx);
    phi=pa.phihk+op.k3*(t-sum(pa.tk(1:5)));
    alp=phi-theat;
end
   
if h<0
    error('h不能小于0')
end 
%大气参数%马赫数
tpra=airCond(h);ma=v_/tpra(4);
Q=qdF(v_,alp,bet,ma,h,theat,sig)/y7(7);%气动加速度
G=gF(t,r_,phiq,x,y,z,vx,vy,vz);%重力加速度，哥氏加速度，离心加速度
P=pF(t,phi,psi)/y7(7);%推力
Ft=P+G;
dydt(1)=Ft(1);
dydt(2)=Ft(2);
dydt(3)=Ft(3);
dydt(4)=y7(1);
dydt(5)=y7(2);
dydt(6)=y7(3);
%质量变化率
if t<pa.tk(1)
    dydt(7)=-pa.mp(1);
elseif t<pa.tk(2)+pa.tk(1)
    dydt(7)=-pa.mp(2);
elseif t<pa.tk(3)+pa.tk(2)+pa.tk(1)
    dydt(7)=-pa.mp(3);
elseif t<sum(pa.tk(1:4))
    dydt(7)=-pa.mp(4);
elseif t<sum(pa.tk(1:5))
    dydt(7)=0;
else
    dydt(7)=-pa.mp(4);
end
%% 弹道数据
cta=Cta(x,y,z,vx,vy,vz);
dad1=[t,v_,x,y,z,vx,vy,vz,alp,phi,h,alp,mass,cta];
dad=[dad;dad1];
rp=r_;%当前近地点
ra=pa.sso+pa.ae;%目标远地点
vp=sqrt(2*pa.fM*ra/rp/(ra+rp));%近地点需求速度
va=sqrt(2*pa.fM*rp/ra/(ra+rp));
if abs(vp-v_)<5 && t>sum(pa.tk(1:3)) && rp<ra%椭圆入轨
    dv=sqrt(v_*v_+vp*vp-2*v_*vp*cos((cta)));
    dv1=pa.vg-va;
    dv2=vp*0.04;%大气损
    dv2=0;
    pa.mtk1=y7(7)*exp(-(dv+dv1+dv2)/3000);
    dad(end,13)=pa.mtk1;
    disp('计算完毕。');
    disp(['时间为：',num2str(t),'s']);
    disp(['高度为：',num2str(h),'m']);
    disp(['质量为：',num2str(dad(end,13)),'kg']);
    disp(['速度为：',num2str(vp),'m/s']);
    error('rugui'); % 终止迭代 
elseif v_>pa.vg&&r_>pa.rg%圆形入轨
    dv2=pa.vg*0.04;%大气损失
    dv2=0;
    dv=sqrt(v_*v_+pa.vg*pa.vg-2*v_*pa.vg*cos(cta));
    pa.mtk1=y7(7)*exp(-(dv+dv2)/3000);
    dad(end,13)=pa.mtk1;
    error('高度，速度满足')
else
    cf=-sqrt((v_-pa.vg)^2+(r_-pa.rg)^2);
    dad(end,13)=cf;%加大惩罚
end
t
end