%Example 5.2 H_inf design for plant with RHP-pole and RHP-zero.
%
clear all
close all

z = 4;
p = 1;  
G = nd2sys([1 -z], conv([1 -p],[0.1 1]));

Wu=1;
M = 2; wb = 1;
Wp = nd2sys([1/M wb], [1 1e-8]); 

systemnames  = 'G Wp Wu';
inputvar     = '[ r(1); u(1)]';
outputvar    = '[Wp; Wu; r-G]';
input_to_G   = '[u]';
input_to_Wp  = '[r-G]';
input_to_Wu  = '[u]';
sysoutname   = 'P';
cleanupsysic = 'yes';
sysic;

nmeas=1; nu=1; gmn=0.5; gmx=20; tol=0.001;
[K,clp,gmfin] = hinfsyn(P,nmeas,nu,gmn,gmx,tol);

L = mmult(G,K); 
S = minv(madd(L,1));  T = msub(1,S);
KS = mmult(-1,K,S);
if all(spoles(T)<0)
   T = sysbal(T);
end
if all(spoles(KS)<0),
   KS = sysbal(KS);
end

hinfnorm(S, 1e-7);
hinfnorm(T, 1e-7);
w = logspace(-2, 2, 401);
Sjw = frsp(S, w); Tjw = frsp(T, w);

yoffs = 0.5;
lp1 = [3e-2 p; 1 p; 2 Inf];
lz1 = [3e-2 z; 1 z; 2 Inf];

%Figure 5.12
subplot(221);
vplot('liv,lm',Sjw,Tjw,1,'w:', lp1, 'w:', lz1, 'w:');
axis([.01,100,.01,4]);
text(.05,.17,'S');
text(.05,1.5,'T');
text(p,1.8e-2,'p');
text(z,1.8e-2,'z');
xlabel('Frequency');
ylabel('Magnitude')

y1 = trsp(T, 1, 5, 0.001);
u1 = trsp(KS, 1, 5, 0.001);
subplot(222);
vplot(y1,mmult(u1,-1),'--',1,'w:');
axis([0,5,-2.1,2.1]);
xlabel('Time');
text(4,1.5,'y');
text(4,0.6,'u');
