%  Example 10.3.  Tracking system design using H-infinity integral control.
clear
%  Define the plant model.
A1=[ 0   1
    -10 -1];
Bw1=[0 0
     0 0];
Bu1=[0 1]';
Cy1=[1 0
     0 0];
Cm1=[-1 0];
Dyw1=zeros(2);
Dyu1=[0 0.01]';
Dmw1=[1 -1];
Dmu1=0;
%  Generate the augmented plant:
epsi=0.01;
A=[  0   1   0    0
    -10 -1   0    0
	 0   0 -epsi  0
	 0   0   0   -10];
Bw=[ 0   0
     0   0
	 1   0
	 0  10];
Bu=[0 1 0 0]';
Cy=[-1 0 1 0
     0 0 0 0];
Cm=[-1 0 1 -1];
Dyw=zeros(2);
Dyu=[0 0.01]';
Dmw=[0 1];
Dmu=0;
Plant=pck(A,[Bw Bu],[Cy;Cm],[Dyw Dyu;Dmw Dmu]);
%  Generate the controller for the augmented model.
K=hinfsyn(Plant,1,1,1100,1100,0.1);
[Ac,Bc,Cc,Dc]=unpck(K);
%  Replace the slow pole in the controller with an integrator.
[Z,P,Gain]=ss2zp(Ac,Bc,Cc,Dc)
[Pmax,index]=max(real(P));
P(index)=0;
[Ac,Bc,Cc,Dc]=zp2ss(Z,P,Gain);
%  Generate the closed loop model.
Acl=[  A1   Bu1*Cc
     Bc*Cm1   Ac  ];
Bcl=[  Bw1
     Bc*Dmw1];
Ccl=[Cy1 Dyu1*Cc];
Dcl=Dyw1;
%  Define the time vector for simulation.
dt=0.01;
t=0:dt:10;
%  Define the inputs for simulating the closed loop system.
r=ones(length(t),1);
w2=(2*rand(length(t),1)-1);
v=lsim(-10,10,-1,1,w2,t);
w=[r v];
%  Simulate the closed loop system.
y=lsim(Acl,Bcl,Ccl,Dcl,w,t);
%  Plot the results.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
subplot(211),plot(t,y(:,1))
xlabel('Time (sec)')
ylabel('Position')
grid
subplot(212),plot(t,y(:,2))
xlabel('Time (sec)')
ylabel('Control')
grid
