clc
clear
n= 2;
A1=[-1,-0.2;0.2,-1]; A2= [-1,0.2 ;-0.2,0.3];
dA=A1-A2;
B=[1;0];
H=[1;0];C=[0 1];
%A = [-1 2 0;-3 -4 1;0 0 -2];
X1 = sdpvar(2,2);
X2 = sdpvar(1,2);
X3 = sdpvar(1,2);
X4 = sdpvar(1,1);
X5 = sdpvar(1,1);
X6 = sdpvar(1,1);
X7 = sdpvar(1,1);
B1=A1'*X1+X1*A1-(C'*X2+X2'*C);
B2=A2'*X1+X1*A2-(C'*X3+X3'*C);
B3=[A2'*X1+X1*A2-(C'*X3+X3'*C)+X6*eye(2),X1*dA+X4*H*H'/2;dA'*X1+X4*H*H'/2,-X4*H*H'-X6*0.01*eye(2)];
B4=[A1'*X1+X1*A1-(C'*X2+X2'*C)+X7*eye(2),-X1*dA+X5*H*H'/2;-dA'*X1+X5*H*H'/2,-X5*H*H'-X7*0.01*eye(2)];
F = [X1 >= 0, B1 <= 0, B2 <= 0, B2 <= 0, B2 <= 0];
F = [F, trace(X1) == 1];
optimize(F);
Pfeasible = value(X1);
L1=(value(X2)/value(X1))'
L2=(value(X3)/value(X1))'