% --------------------------------------
% Reusable Launch Vehicle Entry Example
% --------------------------------------
% This example is taken verbatim from the following reference:
% Betts, J. T., Practical Methods for Optimal Control Using
% Nonlinear Programming, SIAM Press, Philadelphia, 2001.
% Set some auxiliary integer variables to make subsequent coding easier.
imin = 1; imax = 2;
itime = 1; istate = 2; icontrol = 3; iparameter = 4;
ipath = 5; ievent = 6; iduration = 7;
global constants;
constants.Re = 20902900; % Equatorial Radius of Earth (ft)
constants.S = 2690; % Vehicle Reference Area (ft^2)
constants.cl(1) = -0.2070; % Parameters for lift coefficient
constants.cl(2) = 1.6756; % cl(1) and cl(2)
constants.cd(1) = 0.0785; % Parameters for drag coefficient
constants.cd(2) = -0.3529; % cd(1), cd(2), and cd(3)
constants.cd(3) = 2.0400;
constants.b(1) = 0.07854; % Coefficient for heating rate
constants.b(2) = -0.061592; % b(1), b(2), and b(3)
constants.b(3) = 0.00621408;
constants.H = 23800; % Density Scale Height (ft)
constants.al(1) = -0.20704;
constants.al(2) = 0.029244;
constants.rho0 = 0.002378; % Sea Level Atmospheric Density (slug/ft^3)
constants.mu = 1.4076539e16; % Earth Gravitational Parameter (ft^^3/s^2)
constants.mass = 6309.433; %
t0 = 0;
alt0 = 260000;
rad0 = alt0+constants.Re;
lon0 = 0;
lat0 = 0;
speed0 = 25600;
fpa0 = -1*pi/180;
azi0 = 90*pi/180;
tf = 50;
altf = 80000;
radf = altf+constants.Re;
lonf = 90*pi/180;
latf = 30*pi/180;
speedf = 2500;
fpaf = -5*pi/180;
azif = -90*pi/180;
t0min = 0;
t0max = 0;
tfmin = 0;
tfmax = 3000;
radmin = constants.Re;
radmax = rad0;
lonmin = -180*pi/180;;
lonmax = -lonmin;
latmin = -70*pi/180;
latmax = -latmin;
speedmin = 10;
speedmax = 45000;
fpamin = -80*pi/180;
fpamax = 80*pi/180;
azimin = -180*pi/180;
azimax = 180*pi/180;
aoamin = -90*pi/180;
aoamax = -aoamin;
bankmin = -90*pi/180;
bankmax = 1*pi/180;
iphase = 1;
limits{imin}{iphase,itime} = [t0min tfmin];
limits{imax}{iphase,itime} = [t0max tfmax];
limits{imin}{iphase,istate}(1,:) = [rad0 radmin radf];
limits{imax}{iphase,istate}(1,:) = [rad0 radmax radf];
limits{imin}{iphase,istate}(2,:) = [lon0 lonmin lonmin];
limits{imax}{iphase,istate}(2,:) = [lon0 lonmax lonmax];
limits{imin}{iphase,istate}(3,:) = [lat0 latmin latmin];
limits{imax}{iphase,istate}(3,:) = [lat0 latmax latmax];
limits{imin}{iphase,istate}(4,:) = [speed0 speedmin speedf];
limits{imax}{iphase,istate}(4,:) = [speed0 speedmax speedf];
limits{imin}{iphase,istate}(5,:) = [fpa0 fpamin fpaf];
limits{imax}{iphase,istate}(5,:) = [fpa0 fpamax fpaf];
limits{imin}{iphase,istate}(6,:) = [azi0 azimin azimin];
limits{imax}{iphase,istate}(6,:) = [azi0 azimax azimax];
limits{imin}{iphase,icontrol}(1,:) = aoamin;
limits{imax}{iphase,icontrol}(1,:) = aoamax;
limits{imin}{iphase,icontrol}(2,:) = bankmin;
limits{imax}{iphase,icontrol}(2,:) = bankmax;
limits{imin}{iphase,iparameter} = [];
limits{imax}{iphase,iparameter} = [];
limits{imin}{iphase,ipath} = [];
limits{imax}{iphase,ipath} = [];
limits{imin}{iphase,ievent} = [];
limits{imax}{iphase,ievent} = [];
solinit{iphase,itime} = [0; 1000];
solinit{iphase,istate}(:,1) = [rad0; radf];
solinit{iphase,istate}(:,2) = [lon0; lon0];
solinit{iphase,istate}(:,3) = [lat0; lat0];
solinit{iphase,istate}(:,4) = [speed0; speedf];
solinit{iphase,istate}(:,5) = [fpa0; fpaf];
solinit{iphase,istate}(:,6) = [azi0; azif];
solinit{iphase,icontrol}(:,1) = [0; 0];
solinit{iphase,icontrol}(:,2) = [0; 0];
solinit{iphase,iparameter} = [];
setup.name = 'RLV-Entry';
setup.nodes = 40;
setup.limits = limits;
setup.solinit = solinit;
setup.funcs = {'rlvEntryCost','rlvEntryDae',[]};
setup.connections = [];
setup.derivatives = 'complex';
setup.autoscale = 'on';
output = gpops(setup);
solution = output.solution;
