clear
all;
clc
%% Define Matrix
A1 = [0 1; 26.3679 -0.2839]; B1 = [0 -0.2633]'; C1 = [1 0];
A2 = [0 1; 25.7826 -0.2833]; B2 = [0 -0.2469]'; C2 = [1 0];
A3 = [0 1; 24.1023 -0.2818]; B3 = [0 -0.2002]'; C3 = [1 0];
A4 = [0 1; 21.5219 -0.2802]; B4 = [0 -0.1299]'; C4 = [1 0];
Ap = [0 0.0050; 0.1076 0.0014]; Bp = [0 0.0039]'; Cp = [0 0];
Q11 = [0.01 0; 0 0.01]; Q22 = [0.01 0; 0 0.01];
%% Declaration of variables
P22 = sdpvar(2,2);Z1 = sdpvar(2,1); Z2 = sdpvar(2,1); Z3 = sdpvar(2,1); Z4 = sdpvar(2,1);
rho = sdpvar(1,1);
%% Formulation of the LMI matrix
U11 = [P22*A1+A1'*P22-Z1*C1-C1'*Z1'+Ap'*Ap+Q22 P22;
P22 -(3+(1/rho))^-1*eye(2)];
U22 = [P22*A2+A2'*P22-Z2*C2-C2'*Z2'+Ap'*Ap+Q22 P22;
P22 -(3+(1/rho))^-1*eye(2)];
U33 = [P22*A3+A3'*P22-Z3*C3-C3'*Z3'+Ap'*Ap+Q22 P22;
P22 -(3+(1/rho))^-1*eye(2)];
U44 = [P22*A4+A4'*P22-Z4*C4-C4'*Z4'+Ap'*Ap+Q22 P22;
P22 -(3+(1/rho))^-1*eye(2)];
F = [P22>zeros(2)];
F = F+[U11<zeros(4)];
F = F+[U22<zeros(4)];
F = F+[U33<zeros(4)];
F = F+[U44<zeros(4)];
ops = sdpsettings('verbose',1,'warning',0,'showprogress','1','dimacs','1');
sol=solvesdp(F,rho,ops);
solver_time=sol.solvertime;
P22 = double(P22);rho=double(rho);
Z1=double(Z1);Z2=double(Z2);Z3=double(Z3);Z4=double(Z4);
L1 = inv(P22)*Z1;L2 = inv(P22)*Z2;L3 = inv(P22)*Z3;L4 = inv(P22)*Z3;
F = [F, P22 == [1.9318 -0.2116; -0.2116 0.2038]];
sol=solvesdp(F,norm(P22-eye(2)),ops);