Hi, I have a working CVX program given by:
function [output] = RunCVXMissions
for cnt = 1:9
output(cnt) = RunCVX(50,cnt);
end
end
function [output] = RunCVX(varargin)
if nargin == 1
kv = varargin{1};
elseif nargin > 1
kv = varargin{1};
mission = varargin{2};
else
kv = 50;
end
%% 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.
if nargin < 2
xi = [100e3+Re;-0.1278*pi/180;51.5074*pi/180;7450;-0.5*pi/180;-90*pi/180;0]./[Re;1;1;sqrt(Re*g);1;1;1];
xf = [15e3+Re;-74.0060*pi/180;40.7128*pi/180;0;0*pi/180;0;0]./[Re;1;1;sqrt(Re*g);1;1;1];
else
missionic = [[100e3+Re;0*pi/180;0*pi/180;7450;0*pi/180;0*pi/180;0],...
[100e3+Re;0*pi/180;0*pi/180;7450;0*pi/180;0*pi/180;0],...
[100e3+Re;0*pi/180;0*pi/180;7450;0*pi/180;0*pi/180;0],...
[100e3+Re;0*pi/180;0*pi/180;7450;0*pi/180;0*pi/180;0],...
[100e3+Re;0*pi/180;0*pi/180;7450;0*pi/180;0*pi/180;0],...
[100e3+Re;0*pi/180;0*pi/180;7450;0*pi/180;0*pi/180;0],...
[100e3+Re;0*pi/180;0*pi/180;7450;0*pi/180;0*pi/180;0],...
[100e3+Re;0*pi/180;0*pi/180;7450;0*pi/180;0*pi/180;0],...
[100e3+Re;0*pi/180;0*pi/180;7450;0*pi/180;45*pi/180;0]...
]./[Re;1;1;sqrt(Re*g);1;1;1];
missiontc = [[0;0;0;0;0;0;0],...
[0;0*pi/180;65*pi/180;0;0;0;0],...
[0;30*pi/180;50*pi/180;0;0;0;0],...
[0;45*pi/180;35*pi/180;0;0;0;0],...
[0;0;0;0;0;0;0],...
[0e3+Re;45*pi/180;45*pi/180;2500;-25*pi/180;0;0],...
[0e3+Re;45*pi/180;45*pi/180;2500;-25*pi/180;0;0],...
[5e3+Re;45*pi/180;45*pi/180;2500;-25*pi/180;0;0],...
[0;0;0;0;0;0;0]...
]./[Re;1;1;sqrt(Re*g);1;1;1];
xi = missionic(:,mission);
xf = missiontc(:,mission);
end
% Control Matrix.
B = [zeros(6,1);1];
% Virtual control scale.
la = 10^8;
% 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 = [500/Re; 2*pi/180; 2*pi/180; 15/sqrt(Re/g); 2*pi/180; 2*pi/180; 5*pi/180];
% Time parameters
ti = 0; % Initial time in s.
tf = 900; % Final time in s.
N = 150; % 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*pi/180;
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 = 10*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;
for k = 1:kv
pause
cvx_begin
% Variables
variables u(udim,N) ep(xdim,N) x(xdim,N+1)
% expressions al CL CD
x(:,1) == xi;
switch nargin
case 2
% Time parameters
ti = 0; % Initial time in s.
tf = 915; % Final time in s.
% Scaled time parameters.
tin = ti/sqrt(Re/g);
tfn = tf/sqrt(Re/g);
dtn = (tfn-tin)/(N-1);
t = linspace(ti,tf,N+1);
switch mission
case 1
minimize(la*norm(ep,1))
case 2
minimize(la*norm(ep,1))
x(2,N+1) == xf(2); % Longitude
x(3,N+1) == xf(3); % Latitude.
case 3
minimize(la*norm(ep,1))
x(2,N+1) == xf(2); % Longitude
x(3,N+1) == xf(3); % Latitude.
case 4
minimize(la*norm(ep,1))
x(2,N+1) == xf(2); % Longitude
x(3,N+1) == xf(3); % Latitude.
case 5
minimize(-x(2,N+1)*180/pi*1e3 + la*norm(ep,1))
case 6
minimize((-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)))) + la*norm(ep,1))
x(2,N+1) == xf(2); % Longitude
x(3,N+1) == xf(3); % Latitude.
case 7
minimize(-x(4,N+1)*1e3 + la*norm(ep,1))
x(2,N+1) == xf(2); % Longitude
x(3,N+1) == xf(3); % Latitude.
case 8
minimize(la*norm(ep,1))
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.
case 9
minimize(la*norm(ep,1))
x(2,N+1) == xf(2); % Longitude
x(3,N+1) == xf(3); % Latitude.
end
otherwise
minimize(la*norm(ep,1))
x(2,N+1) == xf(2); % Longitude
x(3,N+1) == xf(3); % Latitude.
end
% Path constraints.
for i = 1:N+1
x(1,i) <= xi(1);
x(1,i) >= (500+Re)/Re;
x(4,i) <= 10000/sqrt(g*Re);
x(4,i) >= 500/sqrt(g*Re);
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;
% 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;
% Normal force.
% al = 0.002638.*x(4,i) + 21.11;
% CL = -0.041065 + 0.016292*al + 0.0002602*pow_pos(al,2);
% CD = 0.0009497*pow_pos(al,2) - 0.0183*al + 0.1713;
% -0.5*rel_entr(1,CL^2+CD^2)-rel_entr(1,Re*rho0*Ar/2*m) - (x(1,i)*Re-Re)/(hs) + 2*(-rel_entr(1,rx(4,i))+(1/rx(4,i))*(x(4,i)-rx(4,i))) <= nmax;
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)
if nargin >= 2
fprintf('Convergence of mission %.0f achieved.\n',mission)
else
fprintf('Convergence of condition reached.\n')
end
break
end
end
rx = x;
if strcmp(cvx_status,'Failed') || strcmp(cvx_status,'Infeasible')
if nargin >= 2
fprintf('Optimisation of mission %.0f failed.\n',mission)
else
fprintf('Optimisation failed.\n')
end
break
end
if all(max(max(abs(x(:,i)-rx(:,i)))) <= zi) && ~strcmp(cvx_status,'Inaccurate/Solved')
if nargin >= 2
fprintf('Convergence of mission %.0f achieved.\n',mission)
else
fprintf('Convergence of condition reached.\n')
end
break
end
end
% Vector length fix.
u(:,end+1) = u(:,end);
ep(:,end+1) = ep(:,end);
% Outputs.
output.status = cvx_status;
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;
output.convex.cputtime = cvx_cputime;
output.convex.optval = cvx_optval;
end
function [output] = C(x)
output = StateFunction(x)-Afunc(x)*x;
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 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);
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
I’m finding that if I uncomment the expressions and include the alpha, CL and CD calculations the previously solvable optimization problems produce NaN’s, can anybody tell me what is happening?