%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Example 3.8 Simple 2x2 plant with RHP zero. (Figure 3.9 & 3.10)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

g11 = nd2sys(1,    [0.2 1.2 1]); g12=g11; g22=mmult(g11,2);
g21 = nd2sys([2 1],[0.2 1.2 1]); 

G = abv(sbs(g11,g12),sbs(g21,g22));
spoles(G) % 8 poles
G = sysbal(G); spoles(G) % minimal realization only 4 poles
format short e; szeros(G) % Mu-tools command yields extra zeros at infinity

[A,B,C,D]=unpck(G);
z=tzero(A,B,C,D) % Try Robust control toolbox instead, zero at z=0.5

% Analyze zero direction
G05 = frsp(G,-0.5i)
[u,s,v]=vsvd(G05)  % look at last column in u and v

%Open-loop simulation
figure(1);                    %Figure 3.9
y1=trsp(G,[1;0],10,0.1);
y2=trsp(G,[0;1],10,0.1);
y3=trsp(G,[1;-1],10,0.1);
subplot(231);vplot('iv,d',y1)
subplot(232);vplot('iv,d',y2)
title('OPEN-LOOP SIMULATION')
subplot(233);vplot('iv,d',y3)

% H-INFINITY S-KS design

% Require  wb, slope 1 and peak less than M
M=1.5; wb=0.5*z; wp1 = nd2sys([1/M wb], [1 1.e-6]); wp2=wp1; %weight1
Wp=daug(wp1,wp2); Wu=1.0*eye(2);
systemnames = 'G Wp Wu'
inputvar = '[ r(2); u(2)]';
outputvar = '[Wp; Wu; r-G]';
input_to_G = '[u]';
input_to_Wp = '[r-G]';
input_to_Wu = '[u]';
sysoutname = 'P';
cleanupsysic = 'yes';
sysic;
nmeas=2; nu=2; gmn=0.667; gmx=20; tol=0.001;
[khinf,ghinf,gopt] = hinfsyn(P,nmeas,nu,gmn,gmx,tol); K=khinf;

L = mmult(G,K); i = eye(2); S = minv(madd(i,L)); T=msub(i,S);  
omega=logspace(-3,2,121);
Sf=frsp(S,omega); [u,Sfssav,v]=vsvd(Sf);
% Simulation of bad (y1) and good (y2) direction
y1sav=trsp(T,[1;-1],10,0.1);

% new performance weight

M=1.5; wb=50*z; wp2 = nd2sys([1/M wb], [1 1.e-6]);           %weight2
% when wb2=10 we require much better performance for output 2 
Wp=daug(wp1,wp2); 
systemnames = 'G Wp Wu'
inputvar = '[ r(2); u(2)]';
outputvar = '[Wp; Wu; r-G]';
input_to_G = '[u]';
input_to_Wp = '[r-G]';
input_to_Wu = '[u]';
sysoutname = 'P';
cleanupsysic = 'yes';
sysic;
nmeas=2; nu=2; gmn=0.667; gmx=20; tol=0.001;
[khinf,ghinf,gopt] = hinfsyn(P,nmeas,nu,gmn,gmx,tol); K=khinf;

L = mmult(G,K); S = minv(madd(i,L)); T=msub(i,S);  
Sf=frsp(S,omega); [u,Sfs,v]=vsvd(Sf);
y1=trsp(T,[1;-1],10,0.1);

% Plot
figure(2);            %Figure 3.10
subplot(211);vplot('liv,lm',Sfssav,Sfs,'--',1,':')
title('SINGULAR VALUES OF S:');
text(4,0.02,'DESIGN 1: SOLID LINE');text(4,0.005,'DESIGN 2: DASHED LINE');
axis([0.01,100,0.001,10]);
subplot(212);vplot('iv,d',y1sav,y1,'--',1,':',-1,':')
title('RESPONSE TO CHANGE IN REFERENCE, r =  [1  -1]:');xlabel('Time');
text(4,-1.4,'OUTPUT 2');text(4,1.4,'OUTPUT 1');
axis([0,5,-2,2]);
drawnow;
