clc
clear all
A = [0.858, -0.053, 0.021, 0.193, -0.029, 0.027;
-0.053, 0.716, 0.039, 0.051, 0.179, 0.162;
0.021, 0.039, 0.698, 0.031, 0.135, -0.206;
0.193, 0.051, 0.031, 0.567, 0.053, -0.014;
-0.029, 0.179, 0.135, 0.053, 0.658, -0.031;
0.027, 0.162, -0.206, -0.014, -0.031, 0.504];
B = [-0.438, -1.607, -0.474;
-0.074, -1.479, 1.389;
-1.647, 1.011, 0.204;
0.866, 1.869, 0.821;
0.633, 0.004, -1.364;
-0.997, 1.185, -0.554];
u = [1,1,1]';
nx = size(A,2);
nu = size(B,2);
Q = sdpvar(nx,nx);
Y = sdpvar(nu,nx);
alpha = 1/sqrt(nx);
X = sdpvar(nu,nu);
obj = -log(det(Q));
umax = norm(u,inf);
LMI1 = Q;
LMI2 = [X, Y;
Y', Q];
LMI3 = [alpha^2*Q, (A*Q+B*Y)';
A*Q+B*Y, Q];
% const = [LMI1 >= 0; LMI2 >= 0; LMI3 >= 0, X(1,1) <= u(1)^2, X(2,2) <= u(2)^2, X(3,3) <= u(3)^2];
const = [LMI1 >= 0; LMI2 >= 0; LMI3 >= 0, X(1,1) <= umax^2, X(2,2) <= umax^2, X(3,3) <= umax^2];
% ops = sdpsettings('solver','SeDuMi');
ops = sdpsettings('solver','mosek');
diagnostics = optimize(const, obj, ops);
P = (1/(alpha^2))*inv(value(Q));
M = det(P);..