%Example 2.9, Two degree of freedom controller.
%Produce Figure 2.23 

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

%K3: PID control
c3 = nd2sys(conv([1 2],[0.05,1]),conv([1 1.e-4],[0.005 1]),0.5); 
t3=msub(1,minv(madd(1,mmult(g,c3))));

%Kr: Prefilter for the reference:
cr = nd2sys([0.5 1],conv([0.65 1],[0.03 1])); 

t32 = mmult(t3,cr);  % get Hinf-norm of t32 equal to 1.0
time=0:0.01:3;k=length(time);
[A,B,C,D]=unpck(t3); y3r=step(A,B,C,D,1,time);
[A,B,C,D]=unpck(t32);y32r=step(A,B,C,D,1,time);
Ov3=max([y3r;1]);[x,x,x,tr3]=margin(y3r(2:k)/0.9,y3r(2:k),time(2:k));
Ov32=max([y32r;1]);[x,x,x,tr32]=margin(y32r(2:k)/0.9,y32r(2:k),time(2:k));
disp(sprintf('[Ov3 tr3 Ov32 tr32]=[%5.2f%5.2f%5.2f%5.2f ]',Ov3,tr3,Ov32,tr32));

figure(1);clf;subplot(211);
plot(time,[y32r,y3r],[0,3],[1,1],'w:')
axis([0,3,-0.2,1.5]);
xlabel('Time');ylabel('y');
text(0.3,1.38,'y3');text(0.5,0.85,'y3 (two degrees-of-freedom)');
