% Time Simulation 
addpath(genpath('robust'));

load controller.mat;
[a,b,c,d]=unpck(K);
[n1,d1]=ss2tf(a,b,c,d,1);
[n2,d2]=ss2tf(a,b,c,d,2);
Ktf = [tf(n1(1,:),d1) tf(n1(2,:),d1);tf(n2(1,:),d2) tf(n2(2,:),d2)];

G0 = [87.8 -86.4; 108.2 -109.6];
[U,sval,V]=svd(G0);
dyn = tf(1,[75 1]); 
G=dyn*G0;

I2=eye(2); 

% Nominal
S=minreal(inv(I2+G*Ktf));
T=minreal(I2-S);

% With 20% INPUT uncertainty
Unc = [1.2 0; 0 0.8];
Lu=G*Unc*Ktf;
Su=minreal(inv(I2+Lu));
Tu=minreal(I2-Su);

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),'--');  %Figure 3.12
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)');