%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Remark 3 in Section 3.7.2, S-KS DESIGN
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

G0 = [87.8 -86.4; 108.2 -109.6];
dyn = nd2sys(1,[75 1]); Dyn = daug(dyn,dyn); G = mmult(Dyn,G0);

A= 1.e-4; M=2; wb=0.05;
wp = nd2sys([1/M wb], [1 wb*A]); Wp=daug(wp,wp);
wu = 1; Wu = daug(wu,wu);

systemnames = 'G Wp Wu'
inputvar = '[ r(2); u(2)]';
outputvar = '[Wp; Wu; r-G]';   % NEGATIVE feedback
input_to_G = '[u]';
input_to_Wp = '[r-G]';
input_to_Wu = '[u]';
sysoutname = 'P';
cleanupsysic = 'yes'; sysic;
nmeas=2; nu=2; gmn=1; gmx=100; tol=0.01;
[khinf,ghinf,gopt] = hinfsyn(P,nmeas,nu,gmn,gmx,tol);
K=khinf; 

% TIME  simulation
% Nominal
GK = mmult(G,K); I2=eye(2); S = minv(madd(I2,GK)); T = msub(I2,S);
kr=nd2sys(1,[5 1]); Kr=daug(kr,kr); Tr = mmult(T,Kr);
y = trsp(Tr,[1;0],100,.1);
u = trsp(mmult(K,S,Kr),[1;0],100,.1);
% With 20% uncertainty
Unc = [1.2 0; 0 0.8]; GKu = mmult(G,Unc,K);
Su = minv(madd(I2,GKu)); Tu = msub(I2,Su); Tru = mmult(Tu,Kr);
yu = trsp(Tru,[1;0],100,.1);
uu = trsp(mmult(K,Su,Kr),[1;0],100,.1);
subplot(211);vplot(y,yu,'--');title('OUTPUTS')
subplot(212);vplot(u,uu,'--');title('INPUTS');
xlabel('TIME (min)');
% Simulation shows that the controller is very sensitive
% to uncertainty.  Large and Long peak in y1 and y2.
