function saida = stateFeedback_H2rob(A, B1, B2, C, D2, norma)
% function saida = stateFeedback_H2rob(A, B, C, norma)
%
% Calcula um controlador H-2 robust por realimentaçao de estados  para um sistema linear com
% incerteza politópica usando o seguinte problema de otimização:
%   min Tr(rho)
%   s.a [AiW+WAi'+B2iZ+Z'B2i' B1i] 
%       [B1i'              -I] < 0,
%       
%       [X        CiW+D2iZ]
%       [WCi'+Z'D2i'   W  ] > 0,
%
%       rho > trace(X),
% As LMIs são programadas usando o YALMIP  e podem ser resolvidas
% por qualquer resolvedor de LMIs compatível com o YALMIP (SeDuM é o padrão).
%
% entradas : A  -> vértices da matriz dinâmica do sistema
%            B1 -> vértices da matriz da entrada de ruído do sistema.
%            B2 -> vértices da matriz da entrada de controle do sistema.
%            C  -> vértices da matriz de saída do sistema.
%            D2 -> vértices da matriz de transmissão direta do controle na saída.
%            norma -> limitante da norma a ser testado.
%
% saídas : saida.norma    -> valor da norma (infinito se for infactível)
%          saida.cpusec   -> tempo de cpu para resolver as LMIs (segundos)
%          saida.cpusec_m -> tempo de cpu para programar as LMIs (segundos)
%          saida.V        -> número de variáveis escalares.
%          saida.L        -> number de linhas de LMIs.
%          saida.W        -> variável soluçao W (se existir).
%          saida.K        -> controlador (se existir).
%
% Autor: ricfow@dt.fee.unicamp.br
% Data:  07/09/2010
%
% Exemplo:
% clear all;
% k = [0.09 0.4];
% f = [0.0038 0.04];
% for i=1:2
%     for j=1:2
%         A{2*(i-1)+j} = [0 0 1 0;
%             0 0 0 1;
%             -k(i) k(i) -f(j) f(j);
%             k(i) -k(i) f(j) -f(j)];
%         B1{2*(i-1)+j} = [0;0;0;1];
%         B2{2*(i-1)+j} = [0;0;0;1];
%         C{2*(i-1)+j} = [1 0 0 0;0 1 0 0 ;0 0 0 0];
%         D2{2*(i-1)+j} = [0;0;1];
%     end    
% end
% saida = stateFeedback_H2rob(A, B1, B2, C, D2)

% Determina as dimensões do problema.
ordem    = size(A{1},1);
vertices = size(A,2);
entradas_c = size(B2{1},2);
entradas_r = size(B1{1},2);
saidas     = size(C{1},1);

% Tempo de programação das LMIs.
saida.cpusec_m = clock;

% Inicializa a variável que armazenará as LMIs do problema.
LMIs = set([]);

% Cria as variáveis do problema.
W = sdpvar(ordem,ordem,'symmetric');
Z = sdpvar(entradas_c,ordem,'full');    
X = sdpvar(entradas_r,entradas_r,'symmetric');

if nargin <= 5
    rho = sdpvar(1);
    obj = rho;
else
    rho = norma*norma;
    obj = [];
end

% Entra com as LMIs do problema.
for i=1:vertices

    T11 = A{i}*W+W*A{i}'+B2{i}*Z+Z'*B2{i}';
    T12 = W*C{i}'+Z'*D2{i}';
    T22 = -eye(saidas);
    LMIs = LMIs + set(W > 0) + set([T11 T12;T12' T22]  < 0);

    T11 = X;
    T12 = B1{i}';
    T22 = W;
    LMIs = LMIs + set([T11 T12;T12' T22]  > 0);
end

LMIs = LMIs + set(rho >= trace(X));

% Finaliza o tempo de programação.
saida.cpusec_m = etime(clock,saida.cpusec_m);

% Determina o número de linhas de LMIs
saida.L = 0;
for i=1:size(LMIs,1)
    saida.L = saida.L + size(LMIs{i},1);
end

% Determina o número de variáveis escalares.
saida.V = size(getvariables(LMIs),2);

% Finalmente, o problema de otimização é resolvido.
solucao = solvesdp(LMIs,obj,sdpsettings('solver','sedumi'));
saida.cpusec = solucao.solvertime;

% Acessa o menor resíduo primal.
p=min(checkset(LMIs));
saida.delta = p;

saida.norma = inf;
% Capturando a solução (se existir).
if p > -1e-7
    saida.W     = double(W);    
    saida.norma = sqrt(double(rho));
    saida.K = double(Z)*inv(saida.W);
end