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

clear all;
close all;

G11=tf(87.8,[75 1]);
G12 = tf(-86.4,[75 1]);
G21=tf(108.2,[75 1]);
G22=tf(-109.6,[75 1]);

Gtf=[G11 G12;G21 G22];

% Controlador
Ktf = tf(0.7,[1 0])*inv(Gtf);

S = minreal(inv(eye(2)+Ktf*Gtf));
T = minreal(S*Gtf*Ktf);

Su = minreal(inv(eye(2)+Gtf*[1.2 0; 0 0.8]*Ktf));
Tu = minreal(Su*Gtf*[1.2 0; 0 0.8]*Ktf);

figure
sigma(S,{0.01,100})
hold on
sigma(T,{0.01,100})
legend('S(s)','T(s)')

figure
sigma(Su,{0.01,100})
hold on
sigma(Tu,{0.01,100})
legend('S_u(s)','T_u(s)')


Kr=tf(1,[5 1]); % 5 min filter on reference change
Tr=T*Kr;

u1=[1*ones(1001,1) 0*ones(1001,1)];
t=[0:0.1:100];

y=lsim(Tr,u1,t);
u=lsim(Ktf*S*Kr,u1,t);

Tru=Tu*Kr;
yu=lsim(Tru,u1,t);
uu=lsim(Ktf*Su*Kr,u1,t);

% Plotting
figure
subplot(211); p=plot(t,y(:,1),'-',t,y(:,2),'-',t,yu(:,1),'--',t,yu(:,2),'--');  
text(37,2.3,'Nominal plant: Solid Line ');text(37,2,'Perturbed plant: Dashed Line');
axis([0 60 -0.2 2.7]);text(30,1.2,'y1'),text(30,0.2,'y2');title('OUTPUTS');
subplot(212);plot(t,u,'-',t,uu,'--'),title('INPUTS');figure(1);xlabel('TIME (min)');