# Free Final Time Optimization

I have a working CVX trajectory optimization program given as follows:

``````function [output] = RunCVX(kv)
%% CVX PREP + REF TRAJ
% Constants
Re = 6378e3; % Radius of Earth in m.
g = 9.81;   % Acceleration due to gravity in m/s^2.
kQ = 1.65e-4*sqrt(g*Re)^(3.15); % Heat transfer constant.
rho0 = 1.225; % Density of air in kg/m^3;
hs = 7200; % Reference altitude in m.
Omega = 7.2921150e-5; % Rotational rate of Earth.

% Initial and terminal states (note that these may not necessarily be in
% the cvx program.
xi = [100e3+Re;0;0;7450;-0.5*pi/180;0;0]./[Re;1;1;sqrt(Re*g);1;1;1];
xf = [25e3+Re;0*pi/180;60*pi/180;0;0*pi/180;0*pi/180;0]./[Re;1;1;sqrt(Re*g);1;1;1];

% Control Matrix.
B  = [zeros(6,1);1];

% Virtual control scale.
la = 10^4;

% Trust region and Convergence condition.
de = [10000/Re; 20*pi/180; 40*pi/180; 10000/sqrt(Re/g); 40*pi/180; 60*pi/180; 20*pi/180];
zi = [1500/Re; 2*pi/180; 2*pi/180; 50/sqrt(Re/g); 2*pi/180; 2*pi/180; 5*pi/180];

% Time parameters
ti = 0;    % Initial time in s.
tf = 1000; % Final time in s.
N  = 50;  % Time segments.

% Scaled time parameters.
tin = ti/sqrt(Re/g);
tfn = tf/sqrt(Re/g);
dtn = (tfn-tin)/(N-1);
t = linspace(ti,tf,N+1);

% Vehicle parameters.
m = 104305; % Mass in KG.
Ar = 391.22; % Surface area in m^2.

% Reference trajectory.
rx(:,1) = xi;
rx(:,2:tf*N) = zeros(7,tf*(N)-1);
u(:,1) = 0;
for i = 1:tf*N
rx(:,i+1) = rx(:,i) + dtn/tf*StateFunction(rx(:,i)) + B*u(:,i);
u(:,i+1) = -u(:,i);
end
rxtmp = zeros(7,N);
for i = 1:N
rxtmp(:,i) = rx(:,i*tf);
end
rx = [rxtmp,rxtmp(:,end)];
% Outputs.
output.rx = rx;
output.parameters.initialstate = xi;
output.parameters.terminalstate = xf;
output.t = t;

% Constraints
Qdmax = 1500;
qmax = 18000;
nmax = 2.5*g;
sigmamax = 90*pi/180;

% CVX Setup
xdim = size(rx,1);
udim = 1;

%% CVX
rxs(:,:,1) = rx;
tic
for k = 1:kv
cvx_begin quiet
% Variables
variables u(udim,N) ep(xdim,N) x(xdim,N+1)
% Cost Function
minimize(la*norm(ep(:,:),1))
% Initial and Final conditions.
x(:,1) == xi;
x(1,N+1) == xf(1); % Length from centre of Earth.
x(2,N+1) == xf(2); % Longitude
x(3,N+1) == xf(3); % Latitude.
%     x(5,N+1) == xf(5); % Flight path angle.
%     x(6,N+1) == xf(6); % Heading angle.
% Path constraints.
for i = 1:N+1
x(1,i) <= xi(1);
x(1,i) >= 1+(2500/Re);
x(4,i) <= 1.5;
x(4,i) >= 0.05;
x(5,i) >= -45*pi/180;
x(5,i) <= 45*pi/180;
x(7,i) <= sigmamax;
x(7,i) >= -sigmamax;
end
% Control constraints.
for i = 1:N
end
% Dynamic Constraints.
for i = 1:N
% Heating Rate.
-rel_entr(1,kQ) - 0.5*rel_entr(1,rho0) - (x(1,i)*Re-Re)/(2*hs)+3.15*(-rel_entr(1,rx(4,i))+(1/rx(4,i))*(x(4,i)-rx(4,i))) <= Qdmax;
Qd(i) = 0;%-rel_entr(1,kQ) - 0.5*rel_entr(1,rho0) - (x(1,i)*Re-Re)/(2*hs)+3.15*(-rel_entr(1,rx(4,i))+(1/rx(4,i))*(x(4,i)-rx(4,i)));
% Dynamic pressure.
-rel_entr(1,0.5*g*Re) -rel_entr(1,rho0) - (x(1,i)*Re-Re)/(hs) + 2*(-rel_entr(1,rx(4,i))+(1/rx(4,i))*(x(4,i)-rx(4,i))) <= qmax;
q(i) = 0;%-rel_entr(1,0.5*g*Re) -rel_entr(1,rho0) - (x(1,i)*Re-Re)/(hs) + 2*(-rel_entr(1,rx(4,i))+(1/rx(4,i))*(x(4,i)-rx(4,i)));
% Normal force.
n(i) = 0;
end
% Trust region.
if k > 1
for i = 1:N
abs(x(:,i)-rx(:,i)) <= de;
end
end
% Equations of motion.
for i = 1:N
x(:,i+1) == x(:,i) + dtn*(Afunc(rx(:,i))*x(:,i) + B*u(:,i) + C(rx(:,i)) + ep(:,i));
end
cvx_end
if k > 1
rxs(:,:,k) = rx;
if all(abs(x-rx) < zi)
fprintf('Convergence condition reached.')
break
end
end
rx = x;
if strcmp(cvx_status,'Failed') || strcmp(cvx_status,'Infeasible')
fprintf('Optimisation failed.')
break
end
end
toc
if any(abs(x-rx) > zi)
fprintf('Convergence condition not reached.')
end
% Vector length fix.
u(:,end+1) = u(:,end);
ep(:,end+1) = ep(:,end);
% Outputs.
output.status = cvx_status;
output.constraints.Qd = Qd;
output.constraints.q = q;
output.constraints.n = n;
output.rx = rxs;
output.cost = la*norm(ep,1);
output.x = x;
output.u = u;
output.ep = ep;
output.constraints.heatingrate = Qdmax;
output.constraints.dynamicpressure = qmax;
output.constraints.normalforce = nmax;
end

function A = Afunc(x)
%AF
%    A = AF(X)

%    This function was generated by the Symbolic Math Toolbox version 8.7.
%    04-May-2021 15:25:15

x1 = x(1);
x2 = x(2);
x3 = x(3);
x4 = x(4);
x5 = x(5);
x6 = x(6);
x7 = x(7);

vn = x4*sqrt(9.81*6378e3);
alph = 40*(vn>4570) + ((40-0.20705*(vn-4570^2))/(340^2))*(vn<=4570);

t2 = cos(x3);
t3 = cos(x5);
t4 = cos(x6);
t5 = cos(x7);
t6 = sin(x3);
t7 = sin(x5);
t8 = sin(x6);
t9 = sin(x7);
t10 = tan(x3);
t11 = tan(x5);
t12 = alph.^2;
t13 = x4.^2;
t14 = 1.0./x1;
t17 = 1.0./x4;
t27 = x1.*8.858333333333333e+2;
t30 = alph.*3.66e-2;
t36 = x4.*4.652944151110821;
t39 = x4.*9.468741347510521;
t15 = t14.^2;
t16 = t14.^3;
t18 = 1.0./t13;
t19 = t2.*t3;
t20 = t2.*t7;
t21 = 1.0./t2;
t22 = 1.0./t3;
t24 = -t13;
t25 = t3.*t4.*t6;
t26 = t4.*t6.*t7;
t29 = -t27;
t37 = -t36;
t40 = -t39;
t42 = t12.*7.53e-4;
t23 = t22.^2;
t28 = -t25;
t31 = t14+t24;
t32 = t19+t26;
t34 = t29+8.858333333333333e+2;
t38 = exp(t37);
t41 = exp(t40);
t33 = t20+t28;
t35 = exp(t34);
t43 = t38.*2.664e-1;
t44 = t41.*(2.7e+1./1.0e+2);
t45 = t2.*t32.*x1.*3.457184248754628e-3;
t46 = t30+t43-1.177e-1;
t47 = t42+t44+4.42e-2;
et1 = t17.*(t2.*t8.*1.175956504085866e-1+t3.*t14.*x4.*2.0-t5.*t13.*t35.*t38.*1.85139711293432e+3+t5.*t35.*t46.*x4.*2.987222127180242e+3);
et2 = -t18.*(t45+t2.*t8.*x4.*1.175956504085866e-1+t3.*t14.*(t13-t14)+t5.*t13.*t35.*t46.*1.493611063590121e+3);
et3 = t17.*(t6.*1.175956504085866e-1-t2.*t4.*t11.*1.175956504085866e-1-t9.*t13.*t22.*t35.*t38.*1.85139711293432e+3+t3.*t8.*t10.*t14.*x4.*2.0+t9.*t22.*t35.*t46.*x4.*2.987222127180242e+3);
et4 = -t18.*(x4.*(t6-t2.*t4.*t11).*1.175956504085866e-1+t3.*t8.*t10.*t13.*t14+t9.*t13.*t22.*t35.*t46.*1.493611063590121e+3+t2.*t6.*t8.*t22.*x1.*3.457184248754628e-3);
mt1 = [0.0,-t3.*t8.*t15.*t21.*x4,-t3.*t4.*t15.*x4,t7.*t16.*2.0+t2.*t33.*3.457184248754628e-3+t13.*t35.*t47.*1.323090467163582e+6];
mt2 = [t17.*(t3.*t16+t2.*t32.*3.457184248754628e-3-t3.*t15.*(t13-t14)-t5.*t13.*t35.*t46.*1.323090467163582e+6),-t17.*(t2.*t6.*t8.*t22.*(-3.457184248754628e-3)+t3.*t8.*t10.*t13.*t15+t9.*t13.*t22.*t35.*t46.*1.323090467163582e+6),0.0];
mt3 = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t3.*t6.*t8.*t14.*t21.^2.*x4,0.0,t2.*x1.*(t6.*t7+t4.*t19).*(-3.457184248754628e-3)-t6.*t33.*x1.*3.457184248754628e-3];
mt4 = [-t17.*(t2.*x1.*(t3.*t6-t4.*t20).*3.457184248754628e-3+t6.*t8.*x4.*1.175956504085866e-1+t6.*t32.*x1.*3.457184248754628e-3)];
mt5 = [t17.*(x4.*(t2+t4.*t6.*t11).*1.175956504085866e-1-t6.^2.*t8.*t22.*x1.*3.457184248754628e-3+t8.*1.0./t21.^2.*t22.*x1.*3.457184248754628e-3+t3.*t8.*t13.*t14.*(t10.^2+1.0)),0.0,t7,t3.*t8.*t14.*t21,t3.*t4.*t14];
mt6 = [t13.*t35.*t41.*3.818506545427035e+3-t35.*t47.*x4.*2.987222127180242e+3,et1+et2,et3+et4,0.0,t3.*x4,-t7.*t8.*t14.*t21.*x4,-t4.*t7.*t14.*x4,t45-t3.*t15,-t17.*(t2.*t33.*x1.*3.457184248754628e-3+t7.*t14.*(t13-t14))];
mt7 = [-t17.*(t2.*t4.*x4.*(t11.^2+1.0).*1.175956504085866e-1+t7.*t8.*t10.*t13.*t14-t6.*t8.*t20.*t23.*x1.*3.457184248754628e-3-t7.*t9.*t13.*t23.*t35.*t46.*1.493611063590121e+3),0.0,0.0,t3.*t4.*t14.*t21.*x4,-t3.*t8.*t14.*x4,t6.*t8.*t19.*x1.*3.457184248754628e-3];
mt8 = [t17.*(t2.*t4.*x4.*1.175956504085866e-1-t6.*t8.*t20.*x1.*3.457184248754628e-3),t17.*(t2.*t8.*t11.*x4.*1.175956504085866e-1+t3.*t4.*t10.*t13.*t14+t2.*t4.*t6.*t22.*x1.*3.457184248754628e-3),0.0,0.0,0.0,0.0,0.0];
mt9 = [t9.*t35.*t46.*x4.*(-1.493611063590121e+3),t5.*t22.*t35.*t46.*x4.*1.493611063590121e+3,0.0];
A = reshape([mt1,mt2,mt3,mt4,mt5,mt6,mt7,mt8,mt9],7,7);
end

function [output] = C(x)
output = StateFunction(x)-Afunc(x)*x;
end

function f = StateFunction(x)
%% Input parameters
% radius from centre of Earth = x(1);
% longitude = x(2);
% latitude = x(3);
% velocity = x(4);
% flight path angle = x(5);
% bank angle = x(7);
% Constants
g = 9.81;
Re = 6378e3;
rho0 = 1.225;
hs = 7200;
m = 104305;
Ar = 391.22;
% Calculations
vn = x(4)*sqrt(g*Re);
h = x(1)*Re-Re;
rho = rho0*exp(-h/hs);
omega = 7.2921150e-5/sqrt(g/Re);
Mach = vn/340;
alph = 40*(vn>4570) + ((40-0.20705*(vn-4570)^2)/(340^2))*(vn<=4570);
% Bollino
% CL = 0.0366*alph + 0.2664*exp(-0.2*Mach) - 0.1177;
% CD = 7.53e-4*alph^2 + 0.27*exp(-0.407*Mach) + 0.0442;
% Pei et al.
CL = -0.041065 + 0.016292*alph + 0.00026024*alph^2;
CD = 0.080505 - 0.03026*CL + 0.86495*CL^2;
L = (0.5*Re*rho*x(4)^2*Ar*CL)/(m*g);
D = (0.5*Re*rho*x(4)^2*Ar*CD)/(m*g);

%% Rates.
% Bollino
% rd = x(4)*sin(x(5));
% ld = (x(4)*cos(x(5))*sin(x(6)))/(x(1)*cos(x(3)));
% vd = -D -(sin(x(5))/x(1)^2)+((omega^2*x(1)*cos(x(3)))*(sin(x(5))*cos(x(3))-cos(x(5))*sin(x(3))*cos(x(6))));
% gd = (1/x(4))*(L*cos(x(7))+(x(4)^2-(1/x(1)))*((cos(x(5)))/x(1))+(2*omega*x(4)*cos(x(3))*sin(x(6)))+omega^2*x(1)*cos(x(3))*(cos(x(5))*cos(x(3))+sin(x(5))*cos(x(6))*sin(x(3))));
% pd = (1/x(4))*((L*sin(x(7))/cos(x(5)))+(x(4)^2/x(1))*(cos(x(5))*sin(x(6))*tan(x(3)))-2*omega*x(4)*(tan(x(5))*cos(x(6))*cos(x(3))-sin(x(3)))+((omega^2*x(1))/(cos(x(5))))*(sin(x(6))*sin(x(3))*cos(x(3))));
% sd = 0;
% Pei et al.
rd = x(4)*sin(x(5));
ld = (x(4)*cos(x(5))*sin(x(6)))/(x(1)*cos(x(3)));