Unexpected Infeasibility After Inclusion of Cost Function

1

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?

1 Like
2

CVX reported that the cost function problem is unbounded (which is consistent with the solver reporting the problem to be dual infeasible). CVX and the solver did not find the problem to be (primal) infeasible.

Here are some tips for addressing unbounded problems. https://yalmip.github.io/inside/debuggingunbounded/

I’m not sure I’m looking at this correctly, but is X(:,N+1) essentially unconstrained (despite being on the LHS of a constraint), allowing e(:,N) to be -\infty, which makes the problem unbounded? I’ll let you look into that.

1 Like
3

Hi Mark,

The state is bounded only by the initial and final constraints (though I ought to include an altitude constraint). In future, there will exist path constraints that govern how the various states are bounded (for example the flight-path-angle (x(6,:)) will be bounded via a relationship between it and the velocity. The optimization problem is unbounded since ep is unbounded. The idea is, hopefully, that the solver will always be able to produce a feasible result by taking advantage of the unbounded control “ep”. However, the aim is to include a large cost associated with using this control to reduce its usage as much as possible. Once this cost is included the problem outputs as infeasible. Thank you for the reference I’ll have a look through that and see how I get on.

Thanks, Ayden.

1 Like
4

If there is some problem which is reported as infeasible, you haven’t shown the output. Your current “with objective function” problem is reported as unbounded. e(:,N+1) can be driven to -\infty, so the solver takes advantage of the opportunity to drive the objective value to -\infty, i.e., unbounded.

Do you perhaps want to minimize the norm of vec(e), in some norm of your choosing (1, 2, inf), so that large magnitude elements are penalized, for which e being all zeros is the “perfect” solution, rather than an e element being -\infty being “perfect”? This problem will be feasible and bounded with the current input data.

2 Likes
5

That is the exact problem that I’m having Mark, you’re absolutely right. I couldn’t see the wood for the trees there. Thank you.

1 Like