% Exemplo Hinf sub-ótimo
% Manual Matlab Robust Control Toolbox, pg. 1-60

clear all
close all

% \\\\\\\\\\     Define as funções de ponderação     \\\\\\\\\\

numW1=100*conv([0.005 1],[0.005 1]);     % define W1(s)
denW1=conv([0.2 1],[0.2 1]);
W1=[numW1; denW1];
 
W2=[];                                  % define W2(s) vazia

numW3=[1 0 0];                          % define W3(s)
denW3=[0 0 40000];
%denW3=40000*conv([0.01 1],[0.001 1]);
W3=[numW3;denW3];

% \\\\\\\\\\\     Plota |W1_1(jw)| e |W3_1(jw)|     \\\\\\\\\\

w=logspace(0,3);

% Verificação: 1/|W1|+1/|W3|>1

[magW1,dummy1]=bode(numW1,denW1,w);
[magW3,dummy3]=bode(numW3,denW3,w);

magW1W3inv=[];
for i=1:length(magW1),
    magW1W3inv=[magW1W3inv 20*log10(1/magW1(i)+1/magW3(i))];
end
semilogx(w,magW1W3inv,'r','LineWidth',2)

xlabel('Frequência (rad/s)')
ylabel('Ganho (dB)')
title('Verificação: 1/|W1|+1/|W3| > 1')
grid
pause

figure

[ganhoW1_1,faseW1_1]=bode(denW1,numW1,w);
ganhoW1_1=20*log10(ganhoW1_1);
semilogx(w,ganhoW1_1,'r--','LineWidth',2)
text(2.2,-30,'|W_{1}|^{-1}')
grid
hold
pause

[ganhoW3_1,faseW3_1]=bode(denW3,numW3,w);
ganhoW3_1=20*log10(ganhoW3_1);
semilogx(w,ganhoW3_1,'b--','LineWidth',2)
text(2.2,85,'|W_{3}|^{-1}')

xlabel('Frequência (rad/s)')
ylabel('Ganho (dB)')

pause

% \\\\\\\\\\\   Define a planta nominal G(s)     \\\\\\\\\\

numG=400;
%numG=400*conv([1 1],[0.1 1]);                               % polinômio do numerador de G(s)
denG=[1 2 400];                         % polinômio do denominador de G(s)
[A_G,B_G,C_G,D_G]=tf2ss(numG,denG);     % transforma modelo de G(s) para descrição de estados
sys_G=mksys(A_G,B_G,C_G,D_G);           % cria o sistema sys_G 


% \\\\\\\\\\     Plota o ganho da planta     \\\\\\\\\\\\

[ganhoG,faseG]=bode(numG,denG,w);
ganhoG=20*log10(ganhoG);
semilogx(w,ganhoG,'k','LineWidth',2)
text(10,10,'|G|')
pause

% \\\\\\\\\\     Cria a planta aumentada     \\\\\\\\\\

P=augtf(sys_G,W1,W2,W3);

% \\\\\\\\\\    Resolve o problema Hinf sub-ótimo     \\\\\\\\\\

[ss_K,ss_Ty1u1]=hinf(P);

% \\\\\\\\\\     Monta o modelo de estados do controlador K(s)     \\\\\\\\\\

[A_K,B_K,C_K,D_K]=branch(ss_K);     % extrai o modelo de estados de ss_K

% \\\\\\\\\\\     Plota |S(jw)| e |T(jw)|     \\\\\\\\\\

S=[];
T=[];
[nG,dummy]=size(A_G);
disp('Ordem da planta = ')
disp(num2str(nG))

[nK,dummy]=size(A_K);
disp('Ordem do controlador = ')
disp(num2str(nK))

for i=1:length(w),
    Gi=C_G*inv(j*w(i)*eye(nG)-A_G)*B_G+D_G;
    Ki=C_K*inv(j*w(i)*eye(nK)-A_K)*B_K+D_K;
    GiKi=Gi*Ki;
    Si=1/(1+GiKi);
    S=[S 20*log10(abs(Si))];
    Ti=GiKi*Si;
    T=[T 20*log10(abs(Ti))];
end

semilogx(w,S,'r',w,T,'b','LineWidth',2);
text(2.2,-45,'|S|')
text(2.2,-5,'|T|')
pause

% \\\\\\\\\\\     Plota |W1(jw)|     \\\\\\\\\\

semilogx(w,-ganhoW1_1,'g--','LineWidth',2)
text(2.2,30,'|W_{1}|')

% \\\\\\\\\\     Monta o modelo de malha aberta G(s)K(s)     \\\\\\\\\\

[numK,denK]=ss2tf(A_K,B_K,C_K,D_K);

numGK=conv(numK,numG);
denGK=conv(denK,denG);

% \\\\\\\\\\     Plota |G(jw)K(jw)|     \\\\\\\\\\

[ganhoGK,faseGK]=bode(numGK,denGK,w);
ganhoGK=20*log10(ganhoGK);
semilogx(w,ganhoGK,'g','LineWidth',2)
text(2.2,45,'|GK|')