clear all
load('model.mat')
X=sdpvar(6,6,'full')
P=sdpvar(6,6)
Y=sdpvar(2,6)
S1=sdpvar(6,6)
S2=sdpvar(6,6,'full')
S3=sdpvar(6,6)
inve=sdpvar(1)
rho=sdpvar(1)
alpha=sdpvar(1)
M=sdpvar(12,12)
bigX=blkdiag(X,X);
bigS=[S1,S2;S2',S3];
G11=-S3+d^2*S1+delta1*A*X'+delta1*X*A';
G12=S3+delta1*B*Y+X*A';
G13=-S2';
G14=d^2*S2+P-delta1*X'+X*A'*delta3
G15=delta1*D*inve+X*L';
G16=delta1*Dstar;
G17=X;
G18=zeros(6,2);
G22=-S3+B*Y+Y'*B';
G23=S2';
G24=-X'+delta3*Y'*B';
G25=D*inve;
G26=Dstar;
G27=zeros(6,6);
G28=Y';
G33=-S1;
G34=zeros(6,6);
G35=zeros(6,2);
G36=zeros(6,2);
G37=zeros(6,6);
G38=zeros(6,2);
G44=d^2*S3-delta3*X'-delta3*X;
G45=delta3*D*inve;
G46=delta3*Dstar;
G47=zeros(6,6);
G48=zeros(6,2);
G55=-2*inve*eye(2);
G56=zeros(2,2);
G57=zeros(2,6);
G58=zeros(2,2);
G66=-rho*eye(2);
G67=zeros(2,6);
G68=zeros(2,2)
G77=-inv(Q);
G78=zeros(6,2);
G88=-inv(R)
G1=[G11,G12,G13,G14,G15,G16,G17,G18;
G12',G22,G23,G24,G25,G26,G27,G28;
G13',G23',G33,G34,G35,G36,G37,G38;
G14',G24',G34',G44,G45,G46,G47,G48;
G15',G25',G35',G45',G55,G56,G57,G58;
G16',G26',G36',G46',G56',G66,G67,G68;
G17',G27',G37',G47',G57',G67',G77,G78;
G18',G28',G38',G48',G58',G68',G78',G88;]
G2=[S1,S2;S2',S3];
G3=[alpha,x0';x0,l1*X'+l1*X-l1^2*P];
G4=[M,N';N,l2*bigX+l2*bigX'-l2^2*bigS];
Cons=[G1<0,P>0,G2>0,G3>0,G4>0,S1>0,S3>0,inve>0,rho>0,alpha>0,M>0];
Obs = alpha+trace(M);
% Obs=[]
options = sdpsettings('verbose',1,'solver','sdpt3');
sol = solvesdp(Cons,Obs,options);
% format longEng,
format
if sol.problem == 0
Xopt = double(X)
Yopt = double(Y)
barPopt = double(P)
barS1opt = double(S1)
barS2opt = double(S2)
barS3opt = double(S3)
else
display('Hmm, something went wrong!');
yalmiperror(sol.problem)
end