function saida = stateFeedback_Hinf(A, B1, B2, C, D2, norma)
% function saida = stateFeedback_Hinf(A, B, C, norma)
%
% Calcula um controlador H-infinito por realimentaçao de estados  usando o seguinte problema de 
% otimização:
%   min mu
%   s.a 
% [AW+WA'+B2Z+Z'B2' WC’+Z'D2' B1]
% [CW+D2Z            -I       0 ]
% [B1'                0     -muI] < 0
%       
% 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 -> matriz dinâmica do sistema
%            B1 -> matriz da entrada de ruído do sistema.
%            B2 -> matriz da entrada de controle do sistema.
%            C -> matriz de saída do sistema.
%            D2 -> 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.25;
% f = 0.02;
% A = [0 0 1 0;
% 0 0 0 1;
% -k k -f f;
% k -k f -f];
% B1 = [0;0;0;1];
% B2 = [0;0;0;1];
% C = [1 0 0 0;0 1 0 0 ;0 0 0 0];
% D2 = [0;0;1];
% saida = stateFeedback_Hinf(A, B1, B2, C, D2)

% Determina as dimensões do problema.
ordem      = size(A,1);
entradas_c = size(B2,2);
entradas_r = size(B1,2);
saidas     = size(C,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');    

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

% Entra com as LMIs do problema.
T11= A*W + W*A' + B2*Z + Z'*B2';
T12 = W*C' + Z'*D2';
T13 = B1;
T22 = -eye(saidas);
T23 = zeros(saidas,entradas_r);
T33 = -mu*eye(entradas_r);

LMIs = LMIs + set(W > 0) + set([T11 T12 T13;T12' T22 T23;T13' T23' T33]  < 0);

% 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(mu));
    saida.K = double(Z)*inv(saida.W);
end