%
% Projeto H-infinito
%
% SEM 5928 - Sistemas de Controle 
% Escola de Engenharia de São Carlos - USP
%
% Adriano Siqueira - 2016
%

%close all
clear all
close all

G = nd2sys(1,conv([10 1],conv([0.05 1],[0.05 1])),200);

[Ag,Bg,Cg,Dg]=unpck(G);

% Weighting definition
M = 5.5;
A = 0.0001;
wb = 10;

n=[1/M wb];
d=[1 wb*A];
W1tf = tf(n,d);
W1=nd2sys(n,d);


W2 = 0.5;

% Augmented plant
systemnames='G W1 W2';
inputvar='[r(1); u(1)]';
outputvar='[W1; W2; r-G]';
input_to_G='[u]';
input_to_W1='[r-G]';
input_to_W2='[u]';
sysoutname='P';
cleanupsysic='yes';
sysic

% H-infinity synthesis
gmin=0.5;
gmax=20;
nmeas =1;
nu = 1;
tol = 0.001;
[K1,CL, gopt]=hinfsyn(P,nmeas,nu,gmin,gmax,tol);
[Ak,Bk,Ck,Dk]=unpck(K1);
K=ss(Ak,Bk,Ck,Dk);


% Graphics
[Agk,Bgk,Cgk,Dgk] = series(Ak,Bk,Ck,Dk,Ag,Bg,Cg,Dg);
Lo = ss(Agk,Bgk,Cgk,Dgk);

% Sensitivity function
So = inv(eye(size(Lo))+Lo);


figure
title('Sensitivity, S(s), and W_1^{-1}(s)');


wsigma = logspace(log10(wb*A)-2,log10(wb)+2,100);
SV1 = sigma(So,wsigma);
minSV = min(min(20*log10(SV1)));
maxSV = max(max(20*log10(SV1)));
semilogx(wsigma,20*log10(SV1(1,:)),'g');
hold on
SV = sigma(inv(W1tf),wsigma);
semilogx(wsigma,20*log10(SV),'b');
axis([wsigma(1) wsigma(end) min(min(min(20*log10(SV))),minSV)-10 max(max(max(20*log10(SV))),maxSV)+10])
leg = legend('S (j\omega)','W_{1}^{-1}(j\omega)',2)
set(leg,'FontSize',10)
grid

xla = xlabel('Frequency (rad/s)');
set(xla,'FontSize',10)
yla = ylabel('S(s) and W_{e}^{-1}(s)');
set(yla,'FontSize',10)

% Complementary sensitivity function
KSo = series(K,So);


figure
title('Control Sensitivity, KS(s), and W_u^{-1}(s)');

wsigma = logspace(-5,10,100);
SV1 = sigma(KSo,wsigma);
minSV = min(min(20*log10(SV1)));
maxSV = max(max(20*log10(SV1)));
semilogx(wsigma,20*log10(SV1(1,:)),'g');
hold on
SV = inv(W2)*ones(1,length(wsigma));
semilogx(wsigma,20*log10(SV),'b');

axis([wsigma(1) wsigma(end) min(min(min(20*log10(SV))),minSV)-10 max(max(max(20*log10(SV))),maxSV)+10])
leg = legend('KS (j\omega)','W_2^{-1}(j\omega)',1)
set(leg,'FontSize',10)
grid

xla=xlabel('Frequency (rad/s)');
set(xla,'FontSize',10)
yla=ylabel('KS(s) and W_2^{-1}(s)');
set(yla,'FontSize',10)


figure
To = series(Lo,So);
step(To,3)
title('Malha Fechada')
xla=xlabel('Tempo (s)');
set(xla,'FontSize',10)
yla=ylabel('Saída y');
set(yla,'FontSize',10)

figure
step(KSo,3)

title('Controle')
xla=xlabel('Tempo (s)');
set(xla,'FontSize',10)
yla=ylabel('Controle u');
set(yla,'FontSize',10)