%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Exercise 3.9: Try to robustify inverse-based
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

% Inverse-based controller
dynk = nd2sys([75 1],[1 1.e-6],0.7); Dynk = daug(dynk,dynk);
Kinv = mmult(Dynk,minv(G0)); 

% Try to robustify with respect to coprime uncerainty
Gs = mmult(G,Kinv);
[a,b,c,d]=unpck(Gs);
gamrel=1.1;
[Ac,Bc,Cc,Dc,gammin]=coprimeunc(a,b,c,d,gamrel); % gammin=1.4142
Ks=pck(Ac,-Bc,Cc,-Dc);   % Change from positive to negative feedback
K = mmult(Kinv,Ks);

% 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)');
% Not very successful - almost identical with the original Kinv
