Difficulty Including Constraint

Hello CVX Forum,

I have a working CVX program that is given by the following:

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^5;

% 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 = 800; % 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
    cvx_begin quiet
    % 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
            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 = 40 - 0.20705*(((x(4,i)*sqrt(Re*g))-4570)^2/(340^2));
%         CL(i) = -0.041065 + 0.016292*al + 0.0002602*square(-al);
%         CD(i) =  0.080505 - 0.03026*CL(i) + 0.86495*CL(i)^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.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;
output.convex.cputtime = cvx_cputime;
output.convex.optval = cvx_optval;
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

I’m having a bit of trouble including the “normal force constraint”. For the paper that I am following it determines that the normal force constraint is originally given by:

image

After some logarithmic convexification we can obtain the following:

I can enter the 2nd, 3rd, and 4th coefficients no problem. However, the first coefficient isn’t playing ball! Alpha, and the drag and lift coefficients are given by the following:

image

Now I don’t have any idea how to get alpha to be conditional within CVX so I’ve settled for the “otherwise” and I was going to try to instead have the constraint be conditional and set the value to be whatever the value is when alpha = 40 and to calculate it otherwise. I’m having a little trouble because the non-conditional alpha is concave and it is squared in CL.

Can anybody help?

1 Like

I find your exposition to not be completely clear.

If L and D were variables (or affine expressions in the variables), norm([L D]) <= eta_max would be accepted by CVX. Apparently, your L and D ( C_L, C_D) do not fit these requirements Leaving aside the extra complexity due to the 2 branch definition of alpha, i.e., suppose only the 2nd branch applies, my question to you is have you proven the constraint in question is convex? If it is, perhaps you can model the logic with Big M, If it is not, CVX won’t be able to handle it.

1 Like

Hi Mark,

I’m so sorry that I wasn’t completely clear. L and D are calculated within the state function by the following equations:

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);

The original constraint is non-convex and is given by

image

Taking the logarithm of both sides gives the equation I mentioned before

Now the only dodgy term that I can see is the 2*ln(V), but that can be convexified by linearizing about a previous solution. The CL and CD could be done the same, but the paper does not use this technique.

1 Like

Does the paper claim the problem is convex?

If you do not have a convex formulation, then rather than doing your own linearization, you may be better off using a non-oonvex solver (for instance under YALMIP).

1 Like

The paper does, yes. I think I have it in a convex formulation if I can express CL in some way that the program accepts. It appears that alpha is marked as concave so long as we use the non-conditional version.

1 Like

Can you show us the convex formulation from the paper, and how the paper shows it is convex?

1 Like

Certainly! Here are screenshots from the paper including all the information they give:
image image image image

image .

As you can see the information is pretty sparse. However, so far the results of the paper seem to match up so I’d be surprised if they stopped here.

1 Like

It’s not obvious (in advance) how well any of that stuff will work, to include how robust it is to changes in the input data from the example(s) they ran. You may still be better of using a non-convex solver and letting it handle any linearization (or whatever) internal to the solver.

If you linearize enough stuff, you will get a convex problem. Whether it’s any good is another matter.

1 Like

Right, thank you Mark! I really appreciate your time.

I’m sorry to add to this. The paper actively mentions the use of CVX and I’d love to know how they did it. Do you have any idea?

1 Like

I’'m not clear on what all the linearization performed was.

I suggest emailing the author(s).

1 Like

I believe they use logarithms and the taylor expansion about previous solutions to linearized the equations with a trust-region constraint.

I will do just that!

1 Like