% Simple design example for LMI method described in:
%   K. Zhou, P.P. Khargonekar, J. Stoustrup, and H.H. Niemann.
%   Robust performance of systems with structured uncertainties in state space. 
%   Automatica, 31(2):249-255, February 1995. 
%
% Example is a second order unstable non-minimum phase system with
% uncertain gain and pole.
% 
% Created: 26-04-2010 (Jakob Stoustrup)

%% Definining varying parameters

k = 0.8; % Performance parameter: larger value => higher performance 

p = 2; % No. of parameters
N = 2^p; % No. of vertices

pA_val = [ 1.5 2.5 ];
pB2_val = [ 1 2 ];
pA = zeros(N,1);
pB2 = zeros(N,1);
for I = 0:N-1
    d1 = rem(I,2); % Last binary digit of I
    d2 = rem((I-d1)/2,2); % First binary digit of I
    pA(I+1) = pA_val(d1+1);
    pB2(I+1) = pB2_val(d2+1);
end


%% Generate vertices

D11 = [
    0
    0
    ];

for I = 1:N 
    A{I} = [
        pA(I) 1
        0    -1
        ];
    B1{I} = k*[
        1
        1
        ];
    B2{I} = [
        0
        pB2(I)
        ];
    C1{I} = [
        1 1
        0 0
        ];
    D12{I} = [
        0
        .1
        ];
    sys{I} = ss(A{I},B2{I},C1{I}(1,:),D12{I}(1,:));
end
%%%%%%%%%%%%%%%%%%%%%

figure(1)
bode(sys{1:N})

n = size(A{1},1);
q = size(B1{1},2);
r = size(C1{1},1);
m = size(B2{1},2);

%% Setting up LMIs

% Create matrix (full syntax)
W = sdpvar(m,n,'full');

% Create symmetric matrix (full syntax)
Y = sdpvar(n,n,'symmetric');

R1 = eye(q) - D11'*D11;

for I=1:N
    F11{I} = Y*A{I}' + A{I}*Y +W'*B2{I}' + B2{I}*W;
    F12{I} = B1{I} + Y*C1{I}'*D11 +W'*D12{I}'*D11;
    F13{I} = Y*C1{I}' +W'*D12{I}';

    Phi{I} = [
        F11{I}    F12{I}     F13{I}
        F12{I}'   -R1      zeros(q,r)
        F13{I}' zeros(r,q)  -eye(r)
        ];
end

% Define SETs ...
lmi1 = set('Y>0');
for I=1:N
    lmi1 = lmi1 + set('Phi{I}<0');
end

%% Solve LMIs

% Find feasible solution, minimize trace(Y)
js_options = sdpsettings('solver','lmilab');
solution = solvesdp(lmi1,trace(Y),js_options);

% Extract numerical solution
Y_feas = double(Y)
W_feas = double(W)

% Checking the constraints can also be done with
checkset(lmi1)

F = W_feas/Y_feas;
display('Feedback:')
display(F)

%% Plotting

display('H-infinity norms:')
for I=1:N
    sys_cl{I} = ss(A{I}+B2{I}*F,B1{I},C1{I}+D12{I}*F,D11);
    disp(normhinf(sys_cl{I}))
end

figure(2)
sigma(sys_cl{1:4},{1e-1,1e3})