% sos_gs_cfp.m

% 2011/03/13   H. Ichihara
% 2011/02/03   H. Ichihara

clear all;

% system
adip;
gamma = 148;
ep = 1e-6;

% design specifications
q1 = 5; q2 = 1; 
q3 = 0.01; q4 = 0.01; R = 1;
Q = diag([q1 q2 q3 q4]); Qh = sqrtm(Q);
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);

% constraints
sdpvar rho drho;
g1 = rho*(rho_max-rho); 
g2 = drho_max-drho;
g3 = drho_max+drho;
g4 = rho*(rho0_max-rho);

% Lyapunov variables
n = length(A_0); m = size(B_0,2);
X0 = sdpvar(n); 
X1 = sdpvar(n);
X2 = sdpvar(n);
X = X0+rho*X1+rho^2*X2;
dX = drho*X1+2*rho*drho*X2;

% controller variables
F0 = sdpvar(m,n,'f'); 
F1 = sdpvar(m,n,'f');
F2 = sdpvar(m,n,'f');
F = F0+rho*F1+rho^2*F2;

% matrix SOS polynomials
za = monolist(rho,1);
zb = monolist(rho,1);
zc = monolist(rho,1);
Sa1 = qf_polynomial(za,n); 
Sb1 = qf_polynomial(zb,n);
Sb2 = qf_polynomial(zb,n);
Sb3 = qf_polynomial(zb,n);
Sc1 = qf_polynomial(zc,n);

% SOS conditions
M1 = X;
A = A_0+rho*A_1; B = B_0+rho*B_1;
Phi = A*X+B*F;
M2 = [-(Phi+Phi'-dX) X*Qh       F'*R;
      Qh*X           eye(n)     zeros(n,m);
      R*F            zeros(m,n) R];
M3 = [gamma*eye(n) eye(n);
      eye(n)       X];
M1 = M1-Sa1*g1-ep*eye(n);
M2 = M2-blkdiag(Sb1*g1+Sb2*g2+Sb3*g3,zeros(n),zeros(m))-ep*eye(2*n+m);
M3 = M3-blkdiag(zeros(n),Sc1*g4)-ep*eye(2*n);

% SOS set
Fs = sos(Sa1)+sos(Sb1)+sos(Sb2); 
Fs = Fs+sos(M1)+sos(Sb3)+sos(Sc1)+sos(M2)+sos(M3);
params = recover(setdiff(depends(Fs),depends([rho,drho])));

% call solver
options = sdpsettings;
options.solver = 'sedumi'; % 'lmilab','sdpt3'
if strcmp(options.solver,'lmilab')
 % If you use LMILAB, you may need to change 'options.sos.model'. 
 options.sos.model = 2;
end
[sol,monos,Q0] = solvesos(Fs,[],options,params)
% The 4th output 'resi' of 'solvesos' 
% does not work for matrix SOS. See 'help solvesos'

% check set
for ii=1:length(Fs)
 h = sosd(Fs(ii));
 if max(abs(double(coefficients(Fs{ii}-h'*h,[rho;drho])))) > 1e-6
  disp('Infeasible!!')
 end
end
% 'checkset' does not work correctly 
% for parametrized matrix SOS. An alternative command is 'sosd'.

% EOF