%%% gs_cfp.m
%%% 撌壜夝栤戣

clear; format compact
% ---------------------------
adip
% ---------------------------
q1 = 5;
q2 = 1;
q3 = 0.01;
q4 = 0.01;
R  = 1;
Qh = diag([sqrt(q1) sqrt(q2) sqrt(q3) sqrt(q4)]);
Q = Qh'*Qh;
% ---------------------------
th1_max  = 60*pi/180;
th10_max = 50*pi/180;
drho_max = 1.4;
rho_max  = 1 - cos(th1_max);
rho0_max = 1 - cos(th10_max);
% ---------------------------
n = 4;  p = 1;
X_0 = sdpvar(n,n,'sy');
X_1 = sdpvar(n,n,'sy');
F_0 = sdpvar(p,n,'f');
F_1 = sdpvar(p,n,'f');
gamma = 194;
% ---------------------------
LMI = set([]);
% ---------------------------
rho = [0 1]*rho_max;

for i = 1:2
  X  = X_0 + rho(i)*X_1;
  M1 = X;
  LMI = LMI + set(M1 > 0);
end
% ---------------------------
rho1 = [0 1 0 1]*rho_max;
rho2 = [0 0 1 1]*rho_max^2;
drho = [-1 1]*drho_max;

Phi_0 = A_0*X_0 + B_0*F_0;
Phi_1 = A_1*X_0 + B_1*F_0 ...
      + A_0*X_1 + B_0*F_1;
Phi_2 = A_1*X_1 + B_1*F_1;

for i = 1:4
  for j = 1:2
    Phi = Phi_0 + rho1(i)*Phi_1 + rho2(i)*Phi_2;
    
    X  = X_0 + rho1(i)*X_1;
    F  = F_0 + rho1(i)*F_1;
    dX = drho(j)*X_1;

    M2 = [-(Phi+Phi'-dX)   X*Qh        F'*R
               Qh*X       eye(n)    zeros(n,p)
                R*F      zeros(p,n)     R     ];
    LMI = LMI + set(M2 > 0);
  end
end
% ---------------------------
rho0 = [0 1]*rho0_max;

for i = 1:2
  X_rho0  = X_0 + rho0(i)*X_1;

  M3 = [ gamma*eye(n)  eye(n)
           eye(n)     X_rho0 ];
  LMI = LMI + set(M3 > 0);
end
% ---------------------------
sol = solvesdp(LMI) 
% ---------------------------
if sol.problem ~= 1 
  gamma_opt = double(gamma)
  X_0_opt = double(X_0)
  X_1_opt = double(X_1)
  F_0_opt = double(F_0)
  F_1_opt = double(F_1)
  % -------------------------
  format short e
  pres = checkset(LMI)
  format short
end