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

clear all;
close all;


G11=tf([1],conv([0.2 1],[1 1]));
G12 = G11;
G21=tf([2 1],conv([0.2 1],[1 1]));
G22=tf([2],conv([0.2 1],[1 1]));

Gtf=[G11 G12;G21 G22];

[a,b,c,d]=ssdata(ss(Gtf, 'min'))
n = size(a,1);
mat = [a b;c d];
G = pss2sys(mat,n);

[y1u1,t]=step(Gtf(1,1),7);
[y1u2,t]=step(Gtf(1,2),7);
[y2u1,t]=step(Gtf(2,1),7);
[y2u2,t]=step(Gtf(2,2),7);

figure(1)
plot(t,y1u1,t,y2u1);
legend('y1','y2')
xlabel('Tempo (s)')

figure(2)
plot(t,y1u2,t,y2u2);
legend('y1','y2')
xlabel('Tempo (s)')

figure(3)
plot(t,y1u1-y1u2,t,y2u1-y2u2);
legend('y1','y2')
xlabel('Tempo (s)')


%%% Design 1
M=1.5;wb=0.25;A=1.e-4;

Wp1=nd2sys([1/M wb],[1 wb*A]);
Wp2=nd2sys([1/M wb],[1 wb*A]);

Wp=mmult(Wp1,Wp2);
Wu1=nd2sys([1],[1]);
Wu2=nd2sys([1],[1]);

%Generalized plant P is found with function sysic
systemnames = 'G Wp1 Wp2 Wu1 Wu2';
inputvar = '[r1; r2; u1; u2]';
outputvar = '[Wp1; Wp2; Wu1; Wu2; r1-G(1);r2-G(2)]';
input_to_G= '[u1;u2]';
input_to_Wp1 = '[r1-G(1)]';
input_to_Wp2 = '[r2-G(2)]';
input_to_Wu1 = '[u1]';
input_to_Wu2 = '[u2]';
P=sysic;

%Find H-infinity optimal controller
nmeas=2; nu=2; gmn=0.5; gmx=20;tol=0.001;
[K,CL,gopt]=hinfsyn(P,nmeas,nu,gmn,gmx,tol);

[ag,bg,cg,dg] = unpck(G);
Gss=ss(ag,bg,cg,dg);

[ak,bk,ck,dk] = unpck(K);
Kss=ss(ak,bk,ck,dk);


[awp,bwp,cwp,dwp] = unpck(Wp);
Wpss=ss(awp,bwp,cwp,dwp);


S1=(eye(2)+Gss*Kss)^(-1);
figure(4)
sigma(S1,'b')

% resposta ao degrau de malha fechada
MF=S1*Gss*Kss;
figure(5)
step(S1*Gss*Kss,'b')
title('resposta ao degrau de malha fechada')

[y1u1,t]=step(MF(1,1),18);
[y1u2,t]=step(MF(1,2),18);
figure(6)
plot(t,y1u1-y1u2);
[y2u1,t]=step(MF(2,1),18);
[y2u2,t]=step(MF(2,2),18);
hold on
plot(t,y2u1-y2u2);


%%%% Design 2
M=1.5;wb1=0.25;wb2=25;A=1.e-4;

Wp1=nd2sys([1/M wb1],[1 wb1*A]);
Wp2=nd2sys([1/M wb2],[1 wb2*A]);

Wp=mmult(Wp1,Wp2);
Wu1=nd2sys([1],[1]);
Wu2=nd2sys([1],[1]);


%Generalized plant P is found with function sysic
systemnames = 'G Wp1 Wp2 Wu1 Wu2';
inputvar = '[r1; r2; u1; u2]';
outputvar = '[Wp1; Wp2; Wu1; Wu2; r1-G(1);r2-G(2)]';
input_to_G= '[u1;u2]';
input_to_Wp1 = '[r1-G(1)]';
input_to_Wp2 = '[r2-G(2)]';
input_to_Wu1 = '[u1]';
input_to_Wu2 = '[u2]';
P=sysic;

%Find H-infinity optimal controller
nmeas=2; nu=2; gmn=0.5; gmx=20;tol=0.001;
[K,CL,gopt]=hinfsyn(P,nmeas,nu,gmn,gmx,tol);

[ag,bg,cg,dg] = unpck(G);
Gss=ss(ag,bg,cg,dg);

[ak,bk,ck,dk] = unpck(K);
Kss=ss(ak,bk,ck,dk);


[awp,bwp,cwp,dwp] = unpck(Wp);
Wpss=ss(awp,bwp,cwp,dwp);


S1=(eye(2)+Gss*Kss)^(-1);
figure(4)
hold on
sigma(S1,'r')
hold off
legend('Design 1','Design 2')

% resposta ao degrau de malha fechada
MF=S1*Gss*Kss
figure(5)
hold on
step(S1*Gss*Kss,'r')
hold off
legend('Design 1','Design 2')

[y1u1,t]=step(MF(1,1),18);
[y1u2,t]=step(MF(1,2),18);
figure(6)
plot(t,y1u1-y1u2,'b--');
[y2u1,t]=step(MF(2,1),18);
[y2u2,t]=step(MF(2,2),18);
plot(t,y2u1-y2u2,'g--');

legend('Design 1 - y1','Design 1 - y2','Design 2 - y1','Design 2 - y2')