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
sigmadotmax = 1*pi/180;
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
u(:,i) <= sigmadotmax;
u(:,i) >= -sigmadotmax;
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);
% heading angle = x(6);
% 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)));
% ad = (x(4)*cos(x(5))*cos(x(6)))/(x(1));
% 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)));
ad = (x(4)*cos(x(5))*cos(x(6)))/(x(1));
vd = -D -(sin(x(5))/x(1)^2);
gd = (1/x(4))*(L*cos(x(7)))+((x(4)^2-(1/x(1)))*(cos(x(5))))/(x(1)*x(4));
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))));
sd = 0;
f = [rd;ld;ad;vd;gd;pd;sd];
end
Which can be run via output = RunCVX(kv) where kv is the number of iterations to perform. I have noticed that the output is very sensitive to the terminal time provided. I have verified that this is because the glide vehicle is attempting to either slow down or speed up to make it to the terminal condition at the exact right time which is incredibly difficult. Instead, I wanted to include time as an optimization variable and allow the program to decide on the best final time based on the desired terminal condition. Every paper that I’ve read on the subject argues that this is trivial and therefore not worth disclosing. However, I’m having a lot of difficulty. Can anybody help?