SOCP: sdpvar is identical to zero matrix after several iterations using sdpt3

[xu liu]

Hello,

      I am using yalmip to solve a second-order cone programming problem (reusable vehicle atmospheric re-entry). My code can run for several iterations but then terminate because the decision variable y=sdpvar(7,N) is identical to 0. It seems that the constraints defined in my code constitute a linear programming problem rather than the second-order cone programming problem. How can i revise it? I look forward to your reply. Thank you!

      The Ref.[1] is placed in my google drive [ https://drive.google.com/open?id=1Ka05O-opWbjG_M4ER3sJhI9qZB1Hx36z ]. And my code is as follows:

%%%%%%%

clc; clearvars; close all

% planetary parameters

R0              = 6378000;              %radius

GM            = 3.9866e14;            %gravity parameter

omega0        = 7.292e-5;             %rotate rate

rho0     = 1.225;                %sea level density

H  = 7200;                 %reference altitude

g0                  = GM/R0^2;    %g0

% reusable vehicle

mass = 104305;                              %mass of resuable vehicle

sa   = 391.22;                              %reference area

cl   = [-0.041065, 0.016292, 0.0002602];    %lift coefficient

cd   = [0.080505, -0.03026, 0.86495];       %drag coefficient

aoaParm = [4570; 340];                      %angle of attack

kQ   = 1.5e-4;                              %heat flux coefficient

% initial conditions 

t0 = 0;

alt0 = 100e3;           %altitude

rad0 = alt0+R0;    %

lon0 = deg2rad(0);               %longitude

lat0 = deg2rad(0);               %latitude

speed0 = 7450;          %speed

fpa0   = deg2rad(-0.5); %flight path angle

azi0   = deg2rad(0);    %azimuth

bank0  = deg2rad(0);    %bank

% terminal conditions

altf = 25e3;

radf = altf+R0;

lonf = deg2rad(12);

latf = deg2rad(70);

speedf = 100;

fpaf   = deg2rad(-10);

azif   = deg2rad(90);

bankf  = deg2rad(0);

%state constraints

radmin   = R0;              radmax   = rad0;

lonmin   = deg2rad(0);      lonmax   = pi;

latmin   = deg2rad(0);      latmax   = deg2rad(80);

speedmin = 100;             speedmax = 10000;

fpamin   = deg2rad(-80);    fpamax   = deg2rad(80);

azimin   = deg2rad(0);      azimax   = pi;

bankmin  = deg2rad(0);      bankmax  = deg2rad(80);

%control constraints

umin     = deg2rad(-10);    umax     = -umin;

Qdotmax  = 1500e3;   % w/m^2; w= N*m/s = kg*m/s^2*m/s =kg*m^2/s^3，则w/m^2=kg/s^3

qbarmax  = 18000;    % N/m^2=Pa

nloadmax = 2.5*g0;   % m/s^2

% successive convex optimization

kmax=100;           %Maximum number of SOCP solution iterations

tf=1600;            %simulation time

N=200;              %number of time nodes

dt=(tf-t0)/N;            % 

%

rad1    = linspace(rad0,radf,N);

lon1    = linspace(lon0,lonf,N);

lat1    = linspace(lat0,latf,N);

speed1  = linspace(speed0,speedf,N);

fpa1    = linspace(fpa0,fpaf,N);

azi1    = linspace(azi0,azif,N);

bank1   = linspace(bank0,bankf,N);

x1      = [rad1; lon1; lat1; speed1; fpa1; azi1; bank1];

% k=0

B       = [0 0 0 0 0 0 1].';

Fk = zeros(7,N);  JFk = zeros(7,7,N);

fk = zeros(3,N);  Jfk = zeros(3,2,N);

% state==x，calc Jacobian matrices

syms rad lon lat speed fpa azi bank aoa;

x = [rad; lon; lat; speed; fpa; azi; bank];

CL = cl(1) + cl(2)*aoa + cl(3)*aoa^2;

CD = cd(1) + cd(2)*CL + cd(3)*CL^2;

rho = rho0*exp(-(rad-R0)./H);

L = 0.5*rho*speed^2*sa/mass*CL;

D = 0.5*rho*speed^2*sa/mass*CD;

f1 = kQ*sqrt(rho)*speed^3.15;       % Qdot

f2 = 0.5*rho*speed^2;               % qbar

f3 = sqrt(L^2+D^2);                 % nload

% dynamics equation

F = [speed.*sin(fpa);

    speed.*cos(fpa).*sin(azi)./(rad.*cos(lat));

    speed.*cos(fpa).*cos(azi)./rad;

    -D-sin(fpa).*GM./rad.^2;

    L.*cos(bank)./speed + (speed./rad-GM./rad.^2./speed).*cos(fpa);

    L.*sin(bank)./(speed.*cos(fpa)) + speed.*cos(fpa).*sin(azi).*tan(lat)./rad;

    0];

JF = jacobian(F,x);

% path constraints

f = [f1; f2; f3];

Jf = jacobian(f,[rad; speed]);

for n=1:N

    if x1(4,n)>(aoaParm(1));

        aoa1=40;

    else

        aoa1=40-0.20705*(x1(4,n)-aoaParm(1))^2/aoaParm(2)^2;

    end

    Fk(:,n)    = double(subs(F, [x; aoa],[x1(:,n); aoa1]));

    JFk(:,:,n) = double(subs(JF,[x; aoa],[x1(:,n); aoa1]));

    fk(:,n)    = double(subs(f, [x(1); x(4); aoa],[x1(1,n); x1(4,n); aoa1]));

    Jfk(:,:,n) = double(subs(Jf,[x(1); x(4); aoa],[x1(1,n); x1(4,n); aoa1]));

end

%

delta = [1000; deg2rad(20); deg2rad(20); 500; deg2rad(20); deg2rad(20); deg2rad(20)];% /R0 /speed0

epsilon = [100; deg2rad(0.05); deg2rad(0.05); 10; deg2rad(0.05); deg2rad(0.05); deg2rad(1)];

yk = x1;

objective_value = [];

x_convergence_history = [];

u_convergence_history = [];

options = sdpsettings('solver','sdpt3','verbose',1); % mosek sdpt3

for k = 1:kmax % fail when k=4

    k

    y = sdpvar(7,N);

    u = sdpvar(1,N);

    

    %initial conditions

    constraints = [];

    constraints = [y(:,1)==x1(:,1)];% [rad0; lon0; lat0; speed0; fpa0; azi0; bank0]

    %terminal conditions

    constraints = [constraints;

        diag([1 1 1 0 1 1 1])*(y(:,N)-x1(:,N))==0];% >=diag([1 1 1 0 1 1 1])*epsilon y(1:3,N)==yk(1:3,N); y(5:7,N)==yk(5:7,N)]; % 

    

    %dynamins integration

    for n = 1:N-1

%         %Runge-Kutta integration

%         k1 = Fk(:,n) + JFk(:,:,n)*(y(:,n)-yk(:,n)) + B*u(:,n);

%         k2 = Fk(:,n) + JFk(:,:,n)*(y(:,n)+(dt*k1)/2-yk(:,n)) + B*u(:,n);

%         k3 = Fk(:,n) + JFk(:,:,n)*(y(:,n)+(dt*k2)/2-yk(:,n)) + B*u(:,n);

%         k4 = Fk(:,n) + JFk(:,:,n)*(y(:,n)+dt*k3-yk(:,n)) + B*u(:,n);

%         constraints = [constraints;

%             y(:,n+1) == (y(:,n) + (dt/6)*(k1+ 2*k2 + 2*k3 + k4));];

        

        %Trapezoidal integration

        constraints = [constraints;

            (eye(7)-dt/2*JFk(:,:,n+1))*y(:,n+1)-(eye(7)+dt/2*JFk(:,:,n))*y(:,n)-dt/2*B*u(:,n+1)-dt/2*B*u(:,n)...

            ==dt/2*(Fk(:,n+1)-JFk(:,:,n+1)*yk(:,n+1)+Fk(:,n)-JFk(:,:,n)*yk(:,n))];

    end

    

    %path constraints

    for n = 1:N

        Qdot(1,n) = fk(1,n) + Jfk(1,:,n)*([y(1,n);y(4,n)]-[yk(1,n);yk(4,n)]);

        qbar(1,n) = fk(2,n) + Jfk(2,:,n)*([y(1,n);y(4,n)]-[yk(1,n);yk(4,n)]);

        nload(1,n) = fk(3,n) + Jfk(3,:,n)*([y(1,n);y(4,n)]-[yk(1,n);yk(4,n)]);

    end

    constraints = [constraints;

        Qdot(1,:)<=Qdotmax;

        qbar(1,:)<=qbarmax;

        nload(1,:)<=nloadmax]; % cone(Qdot(1,:),Qdotmax);

    % control and state constraints  

    constraints=[constraints;

        -u(1,:)<=-umin; u(1,:)<=umax;

        -y(1,:)<=-radmin; y(1,:)<=radmax;

        -y(2,:)<=-lonmin; y(2,:)<=lonmax;

        -y(3,:)<=-latmin; y(3,:)<=latmax;

        -y(4,:)<=-speedmin; y(4,:)<=speedmax;

        -y(5,:)<=-fpamin; y(5,:)<=fpamax;

        -y(6,:)<=-azimin; y(6,:)<=azimax;

        -y(7,:)<=-bankmin; y(7,:)<=bankmax];

    

    %optimization

    objective = y(4,N); %minimum ternimal speed

    optimize(constraints,objective,options);

    y_opt = value(y);% y_opt = y;

    u_opt = value(u);% u_opt = u;

%     y = value(y_opt);

%     u = value(u_opt);

    

    %iteration history

    t = linspace(0,tf,N);

    hold on;

    plot(t,x1(1,:)-R0,'k');

    plot(t,y_opt(1,:)-R0);

    xlabel('Time (s)');

    ylabel('Radial Position [m]');

    grid on

    

    % update matrice for the k-th iteration

    Fk = zeros(7,N);  JFk = zeros(7,7,N); fk = zeros(3,N);  Jfk = zeros(3,2,N);

    for n=1:N

        if y_opt(4,n)>(aoaParm(1));

            aoak=40;

        else

            aoak=40-0.20705*(y_opt(4,n)-aoaParm(1))^2/aoaParm(2)^2;

        end

        Fk(:,n)    = double(subs(F, [x; aoa],[y_opt(:,n); aoak])); % Division by zero.

        JFk(:,:,n) = double(subs(JF,[x; aoa],[y_opt(:,n); aoak])); % Division by zero.

        fk(:,n)    = double(subs(f, [x(1); x(4); aoa],[y_opt(1,n); y_opt(4,n); aoak])); % 可以计算

        Jfk(:,:,n) = double(subs(Jf,[x(1); x(4); aoa],[y_opt(1,n); y_opt(4,n); aoak])); % Division by zero.

    end

    

    %convergence history

    objective_value = [objective_value, value(objective)];

    x_convergence_history = [x_convergence_history; y_opt];

    u_convergence_history = [u_convergence_history; u_opt];

    

    %termination

    if all(max(abs(y_opt-yk),[],2)<=epsilon,2)==ones(7,1)

        break

    else

        yk = y_opt; %update yk

    end

end

[Johan Löfberg]

You start with an infeasible problem so of course it fails

Solved in 0 iterations and 0.04 seconds

Infeasible model

ans = 

  struct with fields:

    yalmipversion: '20190425'

       yalmiptime: 0.7420

       solvertime: 0.0530

             info: 'Infeasible problem (GUROBI-GUROBI)'

          problem: 1

https://yalmip.github.io/debugginginfeasible/

other tips is that you have an LP so using SDPT3 is a vast overkill (install gurobi/mosek/cplex), and you can write double-sided inequalities a <= u <= b