-> Solver reported unboundness of the dual problem. -> Your SOS problem is probably infeasible (SOS is dualized). please help me fix this error

addpath(genpath(pwd));

clear;

%%

%パラメータの定義

L = 4.0793; %Boom length [m]

%l = 2.69; %Wire rope length [m]

% l = 1.5;

Theta3 = 0.512410977; %Boom hois angle[rad]

m = 2; %Suspended load[kg]

g = 9.81; %Gravity acceleration[m/s2]

Cb = 2.129; %viscosity damping coefficient of boom

Cl = 0;

I_4b = 7.189; %Moment of inertia around Z-axis

%spring constant

K = 142.83; %サミニ　型番：12-2232S

% K = 10^5; %剛体

%線形モデルの定義（連続時間系）

A0 = [0 0 0 1 0 0;

0 0 0 0 1 0;

0 0 0 0 0 1;

-(g*I_4b+m*g*(L*sin(Theta3))^2)/(I_4b) K*L*sin(Theta3)/(I_4b) -K*L*sin(Theta3)/(I_4b) 0 Cb*L*sin(Theta3)/(I_4b) -Cb*L*sin(Theta3)/(I_4b);

m*g*L*sin(Theta3)/I_4b -K/I_4b K/I_4b 0 -Cb/I_4b Cb/I_4b;

0 0 0 0 0 0];

B0 = [0;0;0;0;0;1];

C0 = [1 0 0 0 0 0;

0 0 1 0 0 0];

D0 = zeros(2,1);

A1 = [0 0 0 0 0 0;

0 0 0 0 0 0;

0 0 0 0 0 0;

(g*I_4b-m*g*(L*sin(Theta3))^2)/(I_4b) -K*L*sin(Theta3)/(I_4b) K*L*sin(Theta3)/(I_4b) 0 -Cb*L*sin(Theta3)/(I_4b) Cb*L*sin(Theta3)/(I_4b);

0 0 0 0 0 0;

0 0 0 0 0 0];

B1 = [0;0;0;0;0;0];

C1 = [0 0 0 0 0 0;

0 0 0 0 0 0];

At_0 = [ A0 zeros(6,2)

-C0 zeros(2,2) ];

At_1 = [ A1 zeros(6,2)

-C1 zeros(2,2) ];

Bt_0 = [ B0

0

0];

Bt_1 = [ B1

0

0];

n = length(At_0);

m = size(Bt_0,2);

Q = diag([100 30 10 200 10 10 1 1]);

R = 10;

la_max = 2.6;

la_min = 0.1;

la0_max = 2.6;

la0_min = 0.1;

w_max = 0.12;

p_max = 1-1/(la_max);

p_min = 1-1/(la_min);

p0_max = 1-1/(la0_max);

p0_min = 1-1/(la0_min);

sdpvar p p0 w;

g1 = (p-p_min)*(p_max - p);

g2 = (p0-p0_min)*(p0_max - p0);

g3a = w_max - w;

g3b = w_max + w;

gamma = sdpvar(1,1);

X_0 = sdpvar(n,n,'symmetric');

X_1 = sdpvar(n,n,'symmetric');

X_2 = sdpvar(n,n,'symmetric');

L_0 = sdpvar(1,n);

L_1 = sdpvar(1,n);

L_2 = sdpvar(1,n);

X = X_0 + p*X_1 + p^2*X_2;

dX = p*X_1 + 2*p*w*X_2;

L = L_0 + p*L_1 + p^2*L_2;

X_p0 = X_0 + p0*X_1 + p0^2*X_2;

z1 = monolist(p,1);

z2 = monolist(p0,1);

z3 = monolist(p,3);

S11 = qf_polynomial(z1,n);

S22 = qf_polynomial(z2,n);

S31 = qf_polynomial(z3,n);

Sa33 = qf_polynomial(z3,n);

Sb33 = qf_polynomial(z3,n);

At = At_0 + p*At_1;

Bt = Bt_0 + p*Bt_1;

M1 = X;

M2 = [ X_p0 eye(n)

eye(n) gamma*eye(n)];

M3 = - [ -dX+(At*X+Bt*L)'+(At*X+Bt*L) X*sqrtm(Q) L'*sqrtm(R)

sqrtm(Q)*X -eye(n) zeros(n,m)

sqrtm(R)*L zeros(m,n) -eye(m) ];

ep = 1e-6;

M1 = M1 - S11*g1 - ep*eye(n);

M2 = M2 - blkdiag(S22*g2+ep*eye(n),zeros(n));

M3 = M3 - blkdiag(S31*g1+Sa33*g3a+Sb33*g3b+ep*eye(n),zeros(n),zeros(m));

CON = [sos(S11),sos(S22),sos(S31),sos(Sa33),sos(Sb33)];

CON = [CON,sos(M1),sos(M2),sos(M3)];

params = recover(setdiff(depends(CON),depends([p0,p,w])));

OBJ = gamma;

solvesos(CON,OBJ,[],params)

gamma_obj = 1.06*double(gamma);

[sol,monos,calQ] = solvesos([CON,gamma<=gamma_obj],[],[],params);

X_0 = double(X_0);

X_1 = double(X_1);

X_2 = double(X_2);

L_0 = double(L_0);

L_1 = double(L_1);

L_2 = double(L_2);

Kt_lq = - lqr(At_0,Bt_0,Q,R);

K1_lq = Kt_lq(1:5);

K2_lq = Kt_lq(6);

num = 0;

for la = linspace(0.1,0.1,2.6)

num = num + 1;

para = 1-1/la;

X = X_0 + para*X_1 + para^2*X_2;

L = L_0 + para*L_1 + para^2*L_2;

K = L*inv(X);

K11(1,num) = K(1);

K12(1,num) = K(2);

K13(1,num) = K(3);

K14(1,num) = K(4);

K15(1,num) = K(5);

K2(1,num) = K(6);

end

% reference, disturbance

t_di = 0;

r1 = 2; k = 1;

% r0 = 45*pi/180; k = 6/8;

% r0 = 30*pi/180; k = 4/8;

d1 = 0;

% ----------------------------

% initial state

theta2_0 = 0;

theta4_0 = 0;

theta5_0 = 0;

dtheta2_0 = 0;

dtheta4_0 = 0;

dtheta5_0 = 0;

% ----------------------------

% J < gamma*x_0^T*x_0 (gamma <= gamma_obj)

gamma_obj

double(gamma)

output

>> Untitled31

-------------------------------------------------------------------------

YALMIP SOS module started...

-------------------------------------------------------------------------

Detected 1989 parametric variables and 3 independent variables.

Detected 0 linear inequalities, 0 equality constraints and 0 LMIs.

Using kernel representation (options.sos.model=1).

Initially 2 monomials in R^1

Newton polytope (0 LPs).........Keeping 2 monomials (0.03125sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 2 monomials in R^1

Newton polytope (0 LPs).........Keeping 2 monomials (0sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 4 monomials in R^1

Newton polytope (0 LPs).........Keeping 4 monomials (0sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 4 monomials in R^1

Newton polytope (0 LPs).........Keeping 4 monomials (0.015625sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 4 monomials in R^1

Newton polytope (0 LPs).........Keeping 4 monomials (0sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 3 monomials in R^1

Newton polytope (0 LPs).........Keeping 3 monomials (0sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 3 monomials in R^1

Newton polytope (0 LPs).........Keeping 3 monomials (0sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 9 monomials in R^2

Newton polytope (2 LPs).........Keeping 5 monomials (0.375sec)

Finding symmetries..............Found no symmetries (0sec)

SeDuMi 1.3.7 by AdvOL, 2005-2008 and Jos F. Sturm, 1998-2003.

Alg = 2: xz-corrector, theta = 0.250, beta = 0.500

eqs m = 3461, order n = 288, dim = 15680, blocks = 10

nnz(A) = 14363 + 0, nnz(ADA) = 6577009, nnz(L) = 3290235

it : b*y gap delta rate t/tP* t/tD* feas cg cg prec

0 : 7.25E-02 0.000

1 : 2.57E-01 4.82E-02 0.000 0.6648 0.9000 0.9000 1.60 1 1 3.0E+00

2 : 7.05E-01 2.28E-02 0.000 0.4720 0.9000 0.9000 2.26 1 1 9.7E-01

3 : 1.34E+00 8.55E-03 0.000 0.3755 0.9000 0.9000 1.13 1 1 3.6E-01

4 : 2.57E+00 2.57E-03 0.000 0.3002 0.9000 0.9000 0.44 1 1 1.7E-01

5 : 4.13E+00 9.19E-04 0.000 0.3582 0.9000 0.9000 0.12 1 1 1.0E-01

6 : 5.53E+00 4.49E-04 0.000 0.4891 0.9000 0.9000 0.07 1 1 7.8E-02

7 : 7.14E+00 2.31E-04 0.000 0.5132 0.9000 0.9000 0.04 1 1 5.9E-02

8 : 8.18E+00 1.53E-04 0.000 0.6652 0.9000 0.9000 -0.06 1 1 5.2E-02

9 : 1.07E+01 7.51E-05 0.000 0.4895 0.9000 0.9000 -0.11 1 1 4.0E-02

10 : 1.32E+01 4.36E-05 0.000 0.5801 0.9000 0.9000 -0.03 1 1 3.1E-02

11 : 1.51E+01 2.79E-05 0.000 0.6414 0.9000 0.9000 -0.11 1 1 2.8E-02

12 : 1.99E+01 1.32E-05 0.000 0.4740 0.9000 0.9000 -0.12 1 1 2.0E-02

13 : 2.44E+01 7.42E-06 0.000 0.5599 0.9000 0.9000 -0.04 2 2 1.6E-02

14 : 2.71E+01 5.23E-06 0.000 0.7057 0.9000 0.9000 -0.08 2 2 1.5E-02

15 : 3.19E+01 3.18E-06 0.000 0.6081 0.9000 0.9000 -0.14 2 2 1.2E-02

16 : 3.97E+01 1.81E-06 0.000 0.5695 0.9000 0.9000 -0.07 2 2 9.7E-03

17 : 4.52E+01 1.15E-06 0.000 0.6341 0.9000 0.9000 -0.13 4 4 8.6E-03

18 : 5.93E+01 5.45E-07 0.000 0.4740 0.9000 0.9000 -0.12 4 4 6.3E-03

19 : 7.02E+01 3.15E-07 0.000 0.5790 0.9000 0.9000 -0.10 5 5 5.3E-03

20 : 8.94E+01 1.58E-07 0.000 0.5025 0.9000 0.9000 -0.07 7 5 4.0E-03

21 : 1.03E+02 1.00E-07 0.000 0.6315 0.9000 0.9000 -0.07 7 7 3.5E-03

22 : 1.24E+02 5.58E-08 0.000 0.5579 0.9000 0.9000 -0.09 18 18 2.8E-03

Run into numerical problems.

iter seconds |Ax| [Ay]_+ |x| |y|

22 51.5 1.4e-03 9.3e-04 2.8e+00 8.5e+01

Failed: no sensible solution/direction found.

Detailed timing (sec)

Pre IPM Post

3.990E-01 4.386E+01 2.099E-02

Max-norms: ||b||=3.735399e-01, ||c|| = 1,

Cholesky |add|=1, |skip| = 16, ||L.L|| = 7750.16.

ans =

struct with fields:

yalmipversion: '20230622'

matlabversion: '9.8.0.1873465 (R2020a) Update 8'

yalmiptime: 6.3713

solvertime: 44.2827

info: 'Numerical problems (learn to debug) (SeDuMi)'

problem: 4

-------------------------------------------------------------------------

YALMIP SOS module started...

-------------------------------------------------------------------------

Detected 1989 parametric variables and 3 independent variables.

Detected 1 linear inequalities, 0 equality constraints and 0 LMIs.

Using kernel representation (options.sos.model=1).

Initially 2 monomials in R^1

Newton polytope (0 LPs).........Keeping 2 monomials (0sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 2 monomials in R^1

Newton polytope (0 LPs).........Keeping 2 monomials (0sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 4 monomials in R^1

Newton polytope (0 LPs).........Keeping 4 monomials (0sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 4 monomials in R^1

Newton polytope (0 LPs).........Keeping 4 monomials (0sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 4 monomials in R^1

Newton polytope (0 LPs).........Keeping 4 monomials (0sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 3 monomials in R^1

Newton polytope (0 LPs).........Keeping 3 monomials (0.03125sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 3 monomials in R^1

Newton polytope (0 LPs).........Keeping 3 monomials (0sec)

Finding symmetries..............Found no symmetries (0sec)

Initially 9 monomials in R^2

Newton polytope (2 LPs).........Keeping 5 monomials (0.23438sec)

Finding symmetries..............Found no symmetries (0sec)

SeDuMi 1.3.7 by AdvOL, 2005-2008 and Jos F. Sturm, 1998-2003.

Alg = 2: xz-corrector, theta = 0.250, beta = 0.500

eqs m = 3462, order n = 289, dim = 15681, blocks = 10

nnz(A) = 14365 + 0, nnz(ADA) = 6581274, nnz(L) = 3292368

it : b*y gap delta rate t/tP* t/tD* feas cg cg prec

0 : 9.69E-02 0.000

1 : 1.12E-02 6.40E-02 0.000 0.6604 0.9000 0.9000 1.32 1 1 3.0E+00

2 : 5.60E-02 3.06E-02 0.000 0.4776 0.9000 0.9000 1.00 1 1 1.6E+00

3 : 3.11E-01 1.15E-02 0.000 0.3750 0.9000 0.9000 0.35 1 1 1.1E+00

4 : 1.52E+00 3.49E-03 0.000 0.3044 0.9000 0.9000 -0.59 1 1 1.1E+00

5 : 5.88E+00 9.25E-04 0.000 0.2651 0.9000 0.9000 -0.84 1 1 1.0E+00

6 : 2.80E+01 1.85E-04 0.000 0.2005 0.9000 0.9000 -0.95 1 1 9.8E-01

7 : 2.69E+02 1.85E-05 0.063 0.0999 0.9900 0.9900 -0.98 1 1 9.4E-01

8 : 8.30E+02 5.88E-06 0.000 0.3176 0.9000 0.9000 -0.99 1 2 9.3E-01

9 : 3.59E+03 1.36E-06 0.000 0.2307 0.9000 0.9000 -1.00 2 2 9.3E-01

10 : 1.63E+04 2.99E-07 0.000 0.2200 0.9000 0.9000 -1.00 3 3 9.2E-01

11 : 7.58E+04 6.58E-08 0.000 0.2205 0.9000 0.9000 -1.00 41 51 9.4E-01

Run into numerical problems.

Primal infeasible, dual improving direction found.

iter seconds |Ax| [Ay]_+ |x| |y|

11 34.4 1.5e-05 2.2e-06 1.4e+01 9.7e+01

Detailed timing (sec)

Pre IPM Post

7.050E-01 2.916E+01 1.301E-02

Max-norms: ||b||=3.735399e-01, ||c|| = 0,

Cholesky |add|=0, |skip| = 373, ||L.L|| = 41.8489.

-> Solver reported unboundness of the dual problem.

-> Your SOS problem is probably infeasible (SOS is dualized).

gamma_obj =

0.1726

ans =

8.8137e-07