% sos_pds.m

% 2011/02/03   H. Ichihara

clear all;

% system
th = sdpvar(2,1);
A = [-2.7-0.5*th(1) 1 -1;
     -12-2*th(2) 2 -1;
     3 1 -18.9-3.5*th(1)-2.7*th(2)-0.5*th(1)*th(2)];
n = length(A);
alpha = 1.0; %2.0453;

% constraints
g1 = alpha^2-th(1)^2;
g2 = alpha^2-th(2)^2;

% Lyapunov variable
zp = monolist(th,1);
P = sm_polynomial(zp,n); 

% matrix SOS polynomials
za = monolist(th,1);
zb = monolist(th,1);
Sa1 = qf_polynomial(za,n);
Sa2 = qf_polynomial(za,n);
Sb1 = qf_polynomial(zb,n);
Sb2 = qf_polynomial(zb,n);

% SOS conditions
ep = 1e-6;
Ca = P-ep*eye(n); 
Ca = Ca-Sa1*g1-Sa2*g2;
Cb = -P*A-A'*P-ep*eye(n);
Cb = Cb-Sb1*g1-Sb2*g2;

% SOS set
Fs = sos(Sa1)+sos(Sa2)+sos(Sb1)+sos(Sb2)+sos(Ca)+sos(Cb);
params = recover(setdiff(depends(Fs),depends(th)));

% 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,Q] = 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,th)))) > 1e-6
  disp('Infeasible!!')
 end
end
% 'checkset' does not work correctly 
% for parametrized matrix SOS. An alternative command is 'sosd'.

% EOF