%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Exercise 3.7 SVD designs 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

[U,sval,V]=svd(G0);
% You must try these one at a time:
c1=0.005; c2=0.005;      % Decentralized: Robust, but poor response
%c1 = 0.005; c2=0.05;    % Good response and also robust
%c1=0.7/197; c2=0.7/1.39;% Inverse-based: Excellent nominal, but not robust

k1 = nd2sys([75 1],[1 1.e-6],c1); k2 = nd2sys([75 1],[1 1.e-6],c2);
Kdiag = daug(k1,k2);
K = mmult(V, Kdiag,U');

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

% Time simulation with higher-order dynamics at the input
dyni1 = nd2sys(1,[0.2 1]); dyni = mmult(dyni1,dyni1,dyni1,dyni1,dyni1);
dyni2 = daug(dyni,dyni); GKu2 = mmult(G,dyni2,Unc,K);
Su2 = minv(madd(I2,GKu2)); Tu2 = msub(I2,Su2);
hinfnorm(Su2), hinfnorm(Tu2)
Tru2 = mmult(Tu2,Kr);
yu2 = trsp(Tru2,[1;0],100,.1);
uu2 = trsp(mmult(K,Su2,Kr),[1;0],100,.1);
subplot(211);vplot(y,yu,'--',yu2,'-.');title('OUTPUTS')
subplot(212);vplot(u,uu,'--',uu2,'-.');title('INPUTS');
% The inverse-based is the only one affected by this higher-order dynamics
