%  Example 11.1.  Reduced order controller approximation.
%  Note:  The "dk_defin.m" file must be inserted into the subs folder within
%    the mutools toolbox before running this program.
clear
%  Define the plant model.
nu=50;de=[conv([1 1],[1 8]) 0];
[A,Bu,Cm,D]=tf2ss(nu,de);
Bw=Bu;
Cy=Cm;
%  Generate a Bode plot of the plant.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
[mag,pha,w]=bode(A,Bu,Cm,D);
subplot(211),semilogx(w,20*log10(mag))
xlabel('Frequency (rad/sec)')
ylabel('Gain (dB)')
grid
subplot(212),semilogx(w,pha)
xlabel('Frequency (rad/sec)')
ylabel('Phase (deg)')
grid
%  Define the bounds.
Ww=0.5;
Wv=0.05;
%  Generate the normalized plant model in standard form.
B1=[Ww*sqrt(2)*Bw zeros(3,1) Bu]; B2=Bu;
C1=[5*Cy; zeros(1,3); zeros(1,3)]; C2=Cm;
D11=zeros(3);
D12=[0;0.01;0.5];D21=[0 Wv*sqrt(2) 0];D22=0;
P=pck(A,[B1 B2],[C1;C2],[D11 D12;D21 D22]);
%  Set variables for DK-iteration algorithm.
  %  Number of measurements.
nmeas=1;
  %  Number of controls.
ncont=1;
  %  Block structure for the perturbation.
blk=[2 2;1 1];
  %  Frequency vector for evaluating the SSV.
omega=logspace(-1,3,30);
%  Perform DK-iteration.
figure(4)
DK_DEF_NAME='my_defin';
dkit
%  The DK-iteration algorithm yields a controllers for each iteration.
%    Request the desired controller.
iter=input('Enter the iteration number of the desired controller ', 's');
K=eval(['k_dk',iter,'suffix']);
%  Generate the state model of the mu-synthesis controller.
[Ac,Bc,Cc,Dc]=unpck(K);
%  Display controller in zero-pole format.
[Z,P,Gain]=ss2zp(Ac,Bc,Cc,Dc);
Gain=-Cc*inv(Ac)*Bc;
[[Z;inf] P]
%
%  Generate the reduced order controller using pole-zero truncation.
%
%  Start with third order since a first order controller can not be 
%  generated due to dominant complex poles.
[Acr3,Bcr3,Ccr3,Dcr3]=zp2ss(Z(9:10),[P(1);P(10:11)],1);
Gainr=-Ccr3*inv(Acr3)*Bcr3;
Bcr3=Bcr3*(Gain/Gainr);
%
%  Fifth order.
[Acr5,Bcr5,Ccr5,Dcr5]=zp2ss([Z(5:6);Z(9:10)],[P(1);P(6:7);P(10:11)],1);
Gainr=-Ccr5*inv(Acr5)*Bcr5;
Bcr5=Bcr5*(Gain/Gainr);
%
%  Generate reduced order controllers using balanced truncation.
%
%  Generate a minimnal realization.
[Am,Bm,Cm,D] = minreal(Ac,Bc,Cc,0);
%  Generate a balanced realization.
[Ab,Bb,Cb,Sigma,T] = balreal(Am,Bm,Cm);
%  Display the singular values of the balanced realization.
Sigma
%  First order controller.
Ac1=Ab(1,1);
Bc1=Bb(1);
Cc1=Cb(1);
Dc1=Dc;
%  Second order controller.
Ac2=Ab(1:2,1:2);
Bc2=Bb(1:2);
Cc2=Cb(1:2);
Dc2=Dc;
%  Third order controller.
Ac3=Ab(1:3,1:3);
Bc3=Bb(1:3);
Cc3=Cb(1:3);
Dc3=Dc;
%  Fourth order controller.
Ac4=Ab(1:4,1:4);
Bc4=Bb(1:4);
Cc4=Cb(1:4);
Dc4=Dc;
%
%  Compare controller transfer functions.
%
%  Define the frequency vector.
w=logspace(0,3);
%  Generate the frequency responses of the controller approximations.
[magc,phac]=bode(Ac,Bc,Cc,Dc,1,w);
[magcr5,phacr5]=bode(Acr5,Bcr5,Ccr5,Dcr5,1,w);
[magc3,phac3]=bode(Ac3,Bc3,Cc3,Dc3,1,w);
[magc4,phac4]=bode(Ac4,Bc4,Cc4,Dc4,1,w);
%  Plot the controller frequency responses.
figure(2)
clf
subplot(211),
semilogx(w,20*log10(magc),'r-',w,20*log10(magcr5),'r--',w,20*log10(magc3),'r-.',w,20*log10(magc4),'r:')
grid
xlabel('Frequecy (rad/sec)')
ylabel('Gain (dB)')
legend('Full','p-z5','bal3','bal4')
subplot(212),semilogx(w,phac,'r-',w,phacr5,'r--',w,phac3,'r-.',w,phac4,'r:')
grid
xlabel('Frequecy (rad/sec)')
ylabel('Phase (deg)')
%
%  Closed loop system performance.
%
%  Generate the full order closed loop system model.
Acl=[ A+Bu*Dc*Cy  Bu*Cc
        Bc*Cy       Ac ];
Bcl=[-Bu*Dc
      -Bc  ];
Ccl=[Cy zeros(size(Cc))];
Dcl=0;
%  Generate the reduced order closed loop system model for the 
%  pole-zero approximations.
%  Third order.
Aclr3=[A+Bu*Dcr3*Cy  Bu*Ccr3
         Bcr3*Cy      Acr3  ];
Bclr3=[-Bu*Dcr3
        -Bcr3  ];
Cclr3=[Cy zeros(size(Ccr3))];
Dclr3=0;
  %  Determine closed loop stability:
  polesr3=eig(Aclr3)
%  Fifth order.
Aclr5=[A+Bu*Dcr5*Cy  Bu*Ccr5
         Bcr5*Cy      Acr5  ];
Bclr5=[-Bu*Dcr5
        -Bcr5  ];
Cclr5=[Cy zeros(size(Ccr5))];
Dclr5=0;
  %  Determine closed loop stability:
  polesr5=eig(Aclr5)
%
%  Generate the reduced order closed loop system model for the 
%  balanced approximation.
%  First order approximation.
Acl1=[A+Bu*Dc1*Cy  Bu*Cc1
        Bc1*Cy      Ac1  ];
Bcl1=[-Bu*Dc1
       -Bc1  ];
Ccl1=[Cy zeros(size(Cc1))];
Dcl1=0;
  %  Determine closed loop stability:
  polesb1=eig(Acl1)
%  Second order approximation.
Acl2=[A+Bu*Dc2*Cy  Bu*Cc2
        Bc2*Cy      Ac2  ];
Bcl2=[-Bu*Dc2
       -Bc2  ];
Ccl2=[Cy zeros(size(Cc2))];
Dcl2=0;
  %  Determine closed loop stability:
  polesb2=eig(Acl2)
%  Third order approximation.
Acl3=[ A+Bu*Dc3*Cy  Bu*Cc3
         Bc3*Cy      Ac3  ];
Bcl3=[-Bu*Dc3
       -Bc3  ];
Ccl3=[Cy zeros(size(Cc3))];
Dcl3=0;
  %  Determine closed loop stability:
  polesb3=eig(Acl3)
%  Fourth order approximation.
Acl4=[ A+Bu*Dc4*Cy  Bu*Cc4
         Bc4*Cy      Ac4  ];
Bcl4=[-Bu*Dc4
       -Bc4  ];
Ccl4=[Cy zeros(size(Cc4))];
Dcl4=0;
  %  Determine closed loop stability:
  polesb4=eig(Acl4)
%  Generate closed loop frequency responses with the controller approximations.
[mag,pha]=bode(Acl,Bcl,Ccl,Dcl,1,w);
[magr5,phar5]=bode(Aclr5,Bclr5,Cclr5,Dclr5,1,w);
[mag3,pha3]=bode(Acl3,Bcl3,Ccl3,Dcl3,1,w);
[mag4,pha4]=bode(Acl4,Bcl4,Ccl4,Dcl4,1,w);
%  Plot the closed loop frequency responses.
figure(3)
clf
subplot(211),
semilogx(w,20*log10(mag),'r-',w,20*log10(magr5),'r--',w,20*log10(mag3),'r-.',w,20*log10(mag4),'r:')
grid
xlabel('Frequecy (rad/sec)')
ylabel('Gain (dB)')
legend('Full','p-z5','bal3','bal4')
subplot(212),semilogx(w,pha,'r-',w,phar5,'r--',w,pha3,'r-.',w,pha4,'r:')
grid
xlabel('Frequecy (rad/sec)')
ylabel('Phase (deg)')
