I have a feasible convex optimization problem that becomes unfeasible when I include a cost function. This surprises me because the cost function shouldn’t rule out the previously feasible solution. I imagine I’m doing something stupid, can anyone figure out what it is?

Here is the code:

```
%% CVX PREP + REF TRAJ
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.
xi = [100e3+Re;0;0;7450;-0.5*pi/180;0;0]./[Re;1;1;sqrt(Re*g);1;1;1];
xf = [25e3+Re;12*pi/180;70*pi/180;0;-10*pi/180;90*pi/180;0]./[Re;1;1;sqrt(Re*g);1;1;1];
B = [zeros(6,1);1];
la = 10^8;
ti = 0; % Initial time in s.
tf = 1600; % Final time in s.
N = tf*5; % Time segments.
dt = (tf - ti)/(N-1); % t delta.
xdim = numel(xi);
udim = 1;
tin = ti/sqrt(Re/g);
tfn = tf/sqrt(Re/g);
dtn = (tfn-tin)/(N-1);
Omegan = Omega/sqrt(g/Re);
t = linspace(ti,tf,N+1);
m = 104305; % Mass in KG.
Ar = 391.22; % Surface area in m^2.
rx(:,1) = xi;
rx(:,2:N) = zeros(7,N-1);
for i = 1:N
rx(:,i+1) = rx(:,i) + dtn*StateFunction(rx(:,i));
end
%% Constraints
sigmadotmax = 10*pi/180;
Qdmax = 1500;
qmax = 18000;
nmax = 2.5*g;
sigmamax = 90*pi/180;
%% CVX
k = 1;
cvx_begin
% Variables
variables u(udim,N) ep(xdim,N) x(xdim,N+1)
% Cost Function
% minimize(sum(sum(ep))) THIS IS UNCOMMENTED IN THE COSTED VERSION!!!
% 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) >= 0;
x(7,i) <= sigmamax;
x(7,i) >= -sigmamax;
u(:,i) <= sigmadotmax;
u(:,i) >= -sigmadotmax;
% log(kQ)+0.5*(log(rho0))-(x(1,i)*Re-Re)/(2*hs)+3.15*(log(rx(4,i))+(1/rx(4,i))*(x(4,i)-rx(4,i))) <= log(Qdmax)
% log(0.5*g*Re)+log(rho0) - (x(1,i)*Re-Re)/(hs) + 2*(log(rx(4,i))+(1/rx(4,i))*(x(4,i)-rx(4,i))) <= log(qmax)
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
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);
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;
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.
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;
f = [rd;ld;ad;vd;gd;pd;sd];
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
```

the output from the uncosted version is as follows:

```
Calling SeDuMi 1.3.4: 127990 variables, 63995 equality constraints
------------------------------------------------------------
SeDuMi 1.3.4 by AdvOL, 2005-2008 and Jos F. Sturm, 1998-2003.
Alg = 2: xz-corrector, Adaptive Step-Differentiation, theta = 0.250, beta = 0.500
Split 79990 free variables
eqs m = 63995, order n = 207981, dim = 207981, blocks = 1
nnz(A) = 511886 + 0, nnz(ADA) = 703813, nnz(L) = 562607
it : b*y gap delta rate t/tP* t/tD* feas cg cg prec
0 : 2.74E-02 0.000
1 : -8.86E+01 2.67E-02 0.000 0.9736 0.9000 0.9000 106.73 1 1 2.0E+02
2 : 6.48E+00 2.29E-02 0.000 0.8578 0.9000 0.9000 126.93 1 1 1.3E+01
3 : 4.79E+00 1.15E-02 0.000 0.5032 0.9000 0.9000 9.16 1 1 1.8E+00
4 : 9.96E-01 2.84E-03 0.000 0.2470 0.9000 0.9000 2.45 1 1 2.7E-01
5 : 2.32E-01 1.13E-03 0.000 0.3979 0.9000 0.9000 1.49 1 1 8.9E-02
6 : 7.85E-02 4.86E-04 0.000 0.4296 0.9000 0.9000 1.20 1 1 3.6E-02
7 : 1.27E-02 1.63E-04 0.000 0.3356 0.9000 0.9000 1.10 1 1 1.2E-02
8 : 7.56E-04 1.15E-05 0.000 0.0705 0.9900 0.9900 1.04 1 1 8.0E-04
9 : 1.17E-06 1.04E-08 0.000 0.0009 0.9990 0.9990 1.00 1 1
iter seconds digits c*x b*y
9 6.1 10.5 0.0000000000e+00 -2.0086166518e-18
|Ax-b| = 3.1e-11, [Ay-c]_+ = 7.3E-18, |x|= 4.6e+02, |y|= 2.3e-16
Detailed timing (sec)
Pre IPM Post
4.612E+00 1.732E+00 1.100E-02
Max-norms: ||b||=6.281080e+02, ||c|| = 0,
Cholesky |add|=0, |skip| = 0, ||L.L|| = 1260.08.
------------------------------------------------------------
Status: Solved
Optimal value (cvx_optval): +0
```

and the costed output is given as follows

```
Calling SeDuMi 1.3.4: 127990 variables, 63995 equality constraints
------------------------------------------------------------
SeDuMi 1.3.4 by AdvOL, 2005-2008 and Jos F. Sturm, 1998-2003.
Alg = 2: xz-corrector, Adaptive Step-Differentiation, theta = 0.250, beta = 0.500
Split 79990 free variables
eqs m = 63995, order n = 207981, dim = 207981, blocks = 1
nnz(A) = 511886 + 0, nnz(ADA) = 703813, nnz(L) = 562607
it : b*y gap delta rate t/tP* t/tD* feas cg cg prec
0 : 1.71E+02 0.000
1 : -3.79E+05 4.26E+01 0.000 0.2493 0.9000 0.9000 -1.51 1 1 1.9E+03
2 : -1.36E+06 1.43E+01 0.000 0.3349 0.9000 0.9000 -1.34 1 1 1.9E+03
3 : -4.43E+06 4.52E+00 0.000 0.3167 0.9000 0.9000 -1.15 1 1 1.9E+03
4 : -1.26E+07 1.45E+00 0.000 0.3196 0.9000 0.9000 -0.93 1 1 1.8E+03
5 : -4.44E+07 3.38E-01 0.000 0.2338 0.9000 0.9000 -0.89 1 1 1.5E+03
6 : -1.56E+08 8.89E-02 0.000 0.2629 0.9000 0.9000 -0.90 1 1 1.5E+03
7 : -6.52E+08 2.07E-02 0.000 0.2328 0.9000 0.9000 -0.97 1 1 1.5E+03
8 : -3.31E+09 4.06E-03 0.000 0.1963 0.9000 0.9000 -0.99 1 1 1.5E+03
9 : -6.45E+11 2.12E-05 0.000 0.0052 0.9990 0.9990 -1.00 1 1
Dual infeasible, primal improving direction found.
iter seconds |Ax| [Ay]_+ |x| |y|
9 4.2 2.8e-13 1.0e-13 4.3e+00 2.9e+02
Detailed timing (sec)
Pre IPM Post
4.620E+00 1.659E+00 1.100E-02
Max-norms: ||b||=6.281080e+02, ||c|| = 1.323503e+00,
Cholesky |add|=0, |skip| = 0, ||L.L|| = 1335.44.
------------------------------------------------------------
Status: Unbounded
Optimal value (cvx_optval): -Inf
```

Can anyone assist with this?