Hello, I’m working through a trajectory optimization paper and I believe I’m coding up the CVX portion incorrectly. I’ve noticed that the number of constraints, linear variables and free variables doesn’t match what I’d expect so I imagine I’m misunderstanding one of the CVX program rules.
Here is my code:
syms r th ph V gam ps sig real
A = jacobian(StateFunction,[r th ph V gam ps sig]);
Af = matlabFunction(A,'Vars',[r,th,ph,V,gam,ps,sig]);
clear r th ph V gam ps sig
%% CVX PREP + REF TRAJ
R0 = 6378e3; % Radius of Earth in m.
g0 = 9.81; % Acceleration due to gravity in m/s^2.
kQ = 1.65e-4*sqrt(g0*R0)^(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.
no = 0;
switch no
case 1
xi = [100e3;0;0;7450;-0.5*pi/180;0;0]./[R0;1;1;sqrt(R0*g0);1;1;1];
xf = [25e3;12*pi/180;70*pi/180;0;-10*pi/180;90*pi/180;0]./[R0;1;1;sqrt(R0*g0);1;1;1];
case 0
xi = [100e3;0;0;7450;-0.5*pi/180;0;0];
xf = [25e3;12*pi/180;70*pi/180;0;-10*pi/180;90*pi/180;0];
end
r = xi(1);
th = xi(2);
ph = xi(3);
V = xi(4);
ga = xi(5);
ps = xi(6);
si = xi(7);
B = [zeros(6,1);1];
la = 10^8;
ti = 0; % Initial time in s.
tf = 1600; % Final time in s.
N = 50; % Time segments.
dt = (tf - ti)/(N-1); % t delta.
xdim = numel(xi);
udim = 1;
ttest = ti:dt:tf;
switch no
case 1
rn = r/R0;
Vn = V/sqrt(R0*g0);
tin = ti/sqrt(R0/g0);
tfn = tf/sqrt(R0/g0);
dtn = (tfn-tin)/(N-1);
Omegan = Omega/sqrt(g0/R0);
case 0
rn = r;
Vn = V;
tin = ti;
tfn = tf;
dtn = (tfn-tin)/(N-1);
Omegan = Omega;
end
m = 104305; % Mass in KG.
Ar = 391.22; % Surface area in m^2.
x(:,1) = [rn;th;ph;Vn;ga;ps;si];
for i = 1:N
xd = StateFunction(x(1,i),x(2,i),x(3,i),x(4,i),x(5,i),x(6,i),x(7,i),no);
x(:,i+1) = x(:,i) + dtn*xd;
end
rx = x;
clear x
%% Constraints
sigmadotmax = 10*pi/180;
Qdmax = 1500;
qmax = 18000;
nmax = 2.5*g0;
sigmamax = 90*pi/180;
%% CVX
k = 1;
cvx_begin
% Variables
variables u(udim,N) ep(xdim,N)
expressions x(xdim,N+1)
% Cost Function
minimize(-x(4,end) + la*sum(sum(ep)));
% Initial and Final conditions.
x(:,1) == xi;
x(1,N) == xf(1); % Length from centre of Earth.
x(2,N) == xf(2); % Longitude
x(3,N) == xf(3); % Latitude.
x(5,N) == xf(5); % Flight path angle.
x(6,N) == xf(6); % Heading angle.
% Path constraints.
for i = 1:N
x(1,i) >= 0;
x(4,i) >= 200/sqrt(R0*g0);
x(5,i) >= -45*pi/180;
x(5,i) <= 90*pi/180;
x(7,i) <= sigmamax;
x(7,i) >= -sigmamax;
u(:,i) <= sigmadotmax;
u(:,i) >= -sigmadotmax;
end
% Equations of motion.
for i = 1:N
x(:,i+1) == x(:,i) + dt*(Af(rx(1,i),rx(2,i),rx(3,i),rx(4,i),rx(5,i),rx(6,i),rx(7,i))*x(:,i) + B*u(:,i) + C(rx(:,i),Af,no) + ep(:,i));
end
cvx_end
function [output] = C(x,Af,no)
output = StateFunction(x(1),x(2),x(3),x(4),x(5),x(6),x(7),no)-Af(x(1),x(2),x(3),x(4),x(5),x(6),x(7))*x;
end
function f = StateFunction(varargin)
switch nargin
case 8
r = varargin{1};
th = varargin{2};
ph = varargin{3};
V = varargin{4};
gam = varargin{5};
ps = varargin{6};
sig = varargin{7};
no = varargin{8};
otherwise
syms V th gam ps r ph sig real
no = 0;
end
R0 = 6378e3;
g0 = 9.8;
hs = 7200;
Aref = 391.22;
switch no
case 1
Omega = 7.2921150e-5/sqrt(g0/R0);
case 0
Omega = 7.2921150e-5;
end
h = r*R0;
hs = 7200;
rho0 = 1.225;
rho = rho0*exp(-h/hs);
m = 104305;
M = (V-4570)^2/(340^2);
alph = 40 - (0.20705*M);
CL = -0.041065 + 0.016292*alph + 0.00026024*alph^2;
CD = 0.080505 - 0.03026*CL + 0.86495*CL^2;
L = R0*rho*V^2*Aref*CL/(2*m);
D = R0*rho*V^2*Aref*CD/(2*m);
Ln = L/g0;
Dn = D/g0;
f = [V*sin(gam);...
(V*cos(gam)*sin(ps))/(r*cos(ph));...
(V*cos(gam)*cos(ps))/r;...
-Dn-(sin(gam))/(r^2)+Omega^2*r*cos(ph)*(sin(gam)*cos(ph)-cos(gam)*sin(ph)*cos(ps));...
((Ln*cos(sig))/(V)) + ((V^2-(1/r))*cos(gam))/(V*r) + 2*Omega*cos(ph)*sin(ps) + Omega^2*r*cos(ph)*(cos(gam)*cos(ph)+sin(gam)*sin(ph)*cos(ps))/V;...
((Ln*sin(sig))/(V*cos(gam))) + (V*cos(gam)*sin(ps)*tan(ph))/r - 2*Omega*(tan(gam)*cos(ps)*cos(ph)-sin(ph)) + (Omega^2*r*sin(ph)*cos(ph)*sin(ps))/(V*cos(gam));...
0];
end
I’ve included everything you should need to just hit run. There is some spaghetti code as I figure out a few different errors. However, the program runs and I’m surprised to see that it is infeasible as I have an unbounded control in eps. The point, as stipulated in the paper, of the eps control vector is that if the problem is infeasible it will find a solution with a very high cost, but will never be infeasible. Additionally, the CVX output states that I have 50 constraints, I see at most 21. It says that I have a dimension of 350 linear variables, which I agree with (7 states discretized into 50 points). Finally, it says that I have 1 dimension of free variables, which may just be the eps, or may be the u but I’m not completely clear. Either way, I’m surprised that the problem can be infeasible given an unbounded control, and am confused about the numbers of constraints, linear variables and free variables.
Additionally, here is the output from the solver for any debuggers.
Calling SDPT3 4.0: 351 variables, 50 equality constraints
For improved efficiency, SDPT3 is solving the dual problem.
------------------------------------------------------------
num. of constraints = 50
dim. of linear var = 350
dim. of free var = 1 *** convert ublk to lblk
*******************************************************************
SDPT3: Infeasible path-following algorithms
*******************************************************************
version predcorr gam expon scale_data
NT 1 0.000 1 0
it pstep dstep pinfeas dinfeas gap prim-obj dual-obj cputime
-------------------------------------------------------------------
0|0.000|0.000|5.0e-09|2.2e+00|1.0e+07|-1.816136e+05 0.000000e+00| 0:0:00| chol 1 1
1|1.000|0.640|1.2e-08|8.1e-01|4.2e+06|-6.823466e+05 0.000000e+00| 0:0:00| chol 1 1
2|1.000|0.068|5.0e-09|7.5e-01|8.8e+06|-7.389778e+07 0.000000e+00| 0:0:00| chol 1 1
3|1.000|0.007|3.3e-09|7.5e-01|9.9e+08|-1.282928e+12 0.000000e+00| 0:0:00| chol 1 1
4|1.000|0.001|2.3e-09|7.5e-01|1.8e+14|-2.235810e+18 0.000000e+00| 0:0:00| chol 1 1
5|1.000|0.000|0.0e+00|7.5e-01|3.9e+21|-3.604800e+26 0.000000e+00| 0:0:00|
sqlp stop: dual problem is suspected of being infeasible
-------------------------------------------------------------------
number of iterations = 5
residual of dual infeasibility
certificate X = 0.00e+00
reldist to infeas. <= 0.00e+00
Total CPU time (secs) = 0.05
CPU time per iteration = 0.01
termination code = 2
DIMACS: 0.0e+00 0.0e+00 1.1e+00 0.0e+00 -1.0e+00 1.1e-05
-------------------------------------------------------------------
------------------------------------------------------------
Status: Infeasible
Optimal value (cvx_optval): +Inf
If somebody could please assist my understanding I’d be very grateful.
Edit: After further testing it seems to think that there is only one constraint per time interval. The linear variable dimension seems to be 7*N where N is the number of time intervals, and the free variable dimension does not change.