%Example 2.11, Produce Figures 2.27 and 2.28.

%Plant (2.54):  
%  G(s)=200/(10s+1)(0.005s+1)^2 ; Gd(s)=100/(10s+1)

gd = nd2sys(1,[10 1],100);
g1 = nd2sys(1,[10 1],200);
g2 = nd2sys(1,[0.05 1]);
g = mmult(g1,g2,g2);

time=0:0.01:3;omega=logspace(-2,2,101);

%weight Wp1
% Require  wb, slope 1 and peak less than 1.5 
M=1.5; wb=10; Wp1 = nd2sys([1/M wb], [1 1.e-6]); 

%weight Wp2
% Require wb, slope 2 and peak less than 1.5 
M=1.5; wb=10; break1 = [1/sqrt(M) wb]; break2 = [1 wb*1.e-3];
wtnum = conv(break1,break1); wtden = conv(break2,break2);
Wp2 = nd2sys(wtnum, wtden); 
Wu = 1;  
G=g;
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';
for i=1:2,
  if i==2,
    cleanupsysic = 'yes';
  else
    cleanupsysic = 'no';
  end
  eval(['Wp=Wp',int2str(i),';']);
  sysic;
  nmeas=1; nu=1; gmn=0.667; gmx=20; tol=0.001;
  [c5,ghinf,gopt] = hinfsyn(P,nmeas,nu,gmn,gmx,tol);
  eval(['c5',int2str(i),'=c5;gopt',int2str(i),'=gopt;']);
end

% RESULTS of H-infinity:
disp(sprintf('%10s%6s%6s%6s%6s%6s%6s','','H_inf','Ms','Mt','GM','PM','Wc'));
for i=1:2,
  eval(['c5=c5',int2str(i),';gopt=gopt',int2str(i),';']);
  l=mmult(g,c5);
  s=minv(madd(1,l)); t=msub(1,s);
  sgd = mmult(s,gd);
  [A,B,C,D]=unpck(sgd);y5=vpck(step(A,B,C,D,1,time),time);
  [A,B,C,D]=unpck(t); y5r=vpck(step(A,B,C,D,1,time),time); 
  eval(['y5',int2str(i),'=y5;y5r',int2str(i),'=y5r;s5',int2str(i),'=s;']);
  [A,B,C,D]=unpck(l);[Gm,Pm,W180,Wc]=margin(A,B,C,D);
  ms=hinfnorm(s);Ms=ms(1);
  mt=hinfnorm(t);Mt=mt(1);
  disp(sprintf('Design%2d: %6.2f%6.2f%6.2f%6.2f%6.1f%6.2f',...
      i,gopt,Ms,Mt,Gm,Pm,Wc));
end

figure(1); 				%Figure 2.27
w1=minv(frsp(Wp1,omega));w2=minv(frsp(Wp2,omega));
s1=frsp(s51,omega);s2=frsp(s52,omega);
subplot(111),vplot('liv,lm',s1,s2,w1,'--',w2,'--',1,':');
axis([.01,100,.0001,10]);
xlabel('Frequency');ylabel('Magnitude')
title('Sensitivity and Inverse of performance weight');
text(0.02,0.0009,'1 / Wp1');text(0.4,.001,'1 / Wp2');
text(0.02, 0.007,'S1');text(.3,.007,'S2');

figure(2); %Firgue 2.28
subplot(221),vplot(y5r1,y5r2,1,':')
axis([0,3,-0.2,1.5]);
xlabel('Time');ylabel('y');
text(0.4,1.35,'y_HINF_2');text(0.4,0.9,'y_HINF_1');
title('Tracking response');
subplot(222),vplot(y51,y52,0,':')
axis([0,3,-0.2,1.5]);
xlabel('Time');ylabel('y');
title('Disturbance response');
text(0.4,0.5,'y_HINF_2');text(1.4,1.0,'y_HINF_1');
