%HINFKGJD Continuous H-Infinity control synthesis (Glover/Doyle formulae).
%
%     << 4-block L-inf optimal Controller (all solution formulae) >>
%               (derived by K. Glover & J. Doyle, 1988)
%
%   HINFKGJD is a script file which computes the H-inf optimal controller
%   using G-D's all-solution "two-Riccati" formulae.
%   A particular controller F(s) is returned by default, however the
%   other solution can be generated by prespecifing the transfer function
%   in the workspace
%                       U(s):= (AU,BU,CU,DU)
%   where U(s) is a free stable contraction which parameterizes all the
%   stabilizing controllers.
%
%   Input data (must be in Upper Case !!!):
%              augmented plant (A,B1,B2,C1,C2,D11,D12,D21,D22)
%              stable contraction U(s):= (AU,BU,CU,DU) (optional)
%   Output data: H-inf controller F(s):= (acp,bcp,ccp,dcp),
%                CLTF Ty1u1:= (acl,bcl,ccl,dcl).
%

% R. Y. Chiang & M. G. Safonov 2/10/88
% Copyright 1988-2000 The MathWorks, Inc. 
%       $Revision: 1.5 $
% All Rights Reserved.
% ------------------------------------------------------------------------
disp('   ')
disp('                << H-inf Optimal Control Synthesis >>')
disp(' ')
disp(' - - - Computing 4-block L-inf optimal controller ')
disp('       (using the all-solution formulae of Glover and Doyle) - - -')
%
flagcase1 = exist('b1');
flagcase2 = exist('B1');
if (flagcase1 > 0) & (flagcase2 < 1)
 error('   THIS SCRIPT FILE REQUIRES THE INPUT VARIABLE NAMES IN UPPER CASE !!!')
end
%
FLAG = exist('AU');
if FLAG < 1
   disp('  ')
   disp(' - - - Solving a particular solution F(s) (U(s) = 0 case) - - -');
end
if FLAG > 0
   disp('  ')
   disp(' - - - Solving the all-solution F(s) (U(s):=(AU,BU,CU,DU)) - - -');
end
AREFLAG = exist('aretype');
%
% ------------------------------------------- Pre-process the augmented plant:
%
% ------------------------------------------- 1) Zero out D22 term:
%
[mD22,nD22] = size(D22);
d22 = zeros(mD22,nD22);
%
% ------------------------------------------- 2) Find the scaling factors:
%
[n,n] = size(A);
[p1,m2] = size(D12);
[uu,sig12,vv] = svd(D12);
u1 = uu(:,1:m2);
u2 = uu(:,m2+1:p1);
su = vv*inv(sig12(1:m2,1:m2));
sz = [u2';u1'];
%
[p2,m1] = size(D21);
[uuu,sig21,vvv] = svd(D21);
v1 = vvv(:,1:p2);
v2 = vvv(:,p2+1:m1);
sy = inv(sig21(1:p2,1:p2))*uuu';
sw = [v2 v1];
%
b1  = B1*sw;
b2  = B2*su;
c1  = sz*C1;
c2  = sy*C2;
d11 = sz*D11*sw;
d21 = sy*D21*sw;
d12 = sz*D12*su;
%
% ----------------------------------------- 3) Check frequency @ infinity:
%
d1111 = d11(1:p1-m2,1:m1-p2);
d1121 = d11(p1-m2+1:p1,1:m1-p2);
d1112 = d11(1:p1-m2,m1-p2+1:m1);
d1122 = d11(p1-m2+1:p1,m1-p2+1:m1);
mxsvd11r = max(svd([d1111 d1112]));
mxsvd11c = max(svd([d1111;d1121]));
if mxsvd11r >= 1 | mxsvd11c >= 1
     disp('  ')
     disp('    ------------------------------------------------------------');
     disp('       NO CONTROLLER MEETS THE SPEC. EVEN AT FREQ. INF ALONE !!')
     disp('       ADJUST "Gam" AND TRY AGAIN ...')
     disp('    ------------------------------------------------------------');
     break
end
%
% ----------------------------------------------------------------------------
%
% ----------------------------------------- Start to compute the controller:
gam = 1;
gam2 = gam*gam;
%
% ----------------------------------------- Fixing D11 matrix:
%
if m1 == p2 | p1 == m2
   dk11 = -d1122;
   dk12 = eye(m2);
   dk21 = eye(p2);
else
   gamd1  = gam2*eye(p1-m2)-d1111*d1111';
   gamd2  = gam2*eye(m1-p2)-d1111'*d1111;
   dk11   = -d1121*d1111'*inv(gamd1)*d1112-d1122;
   dk1212 = eye(m2) - d1121*inv(gamd2)*d1121';
   dk12   = chol(dk1212); dk12 = dk12';
   dk2121 = eye(p2) - d1112'*inv(gamd1)*d1112;
   dk21   = chol(dk2121);
end
%
% ---------------------------------------- Solving X Riccati:
%
Zr = zeros(m1+m2,m1+m2);
Zr(1:m1,1:m1) = gam2*eye(m1);
d1d = [d11 d12];
R = d1d'*d1d - Zr;
Ri = inv(R);
B = [b1 b2];
ax = A-B*Ri*d1d'*c1;
rx = B*Ri*B';
qx = c1'*c1 - c1'*d1d*Ri*d1d'*c1;
%
disp(' ');
disp(' - - - Solving the 1st Riccati - - -');
if AREFLAG > 0
   [X1,X2,lamXc,Xerr,wellposed,X] = aresolv(ax,qx,rx,aretype);
else
   [X1,X2,lamXc,Xerr,wellposed,X] = aresolv(ax,qx,rx);
end
%
% ---------------------------------------- Solving Y Riccati:
%
Zrw = zeros(p1+p2,p1+p2);
Zrw(1:p1,1:p1) = gam2*eye(p1);
dd1 = [d11;d21];
Rw = dd1*dd1' - Zrw;
Rwi = inv(Rw);
C = [c1;c2];
ay = A' - C'*Rwi*dd1*b1';
ry = C'*Rwi*C;
qy = b1*b1' - b1*dd1'*Rwi*dd1*b1';
disp('   ');
disp(' - - - Solving the 2nd Riccati - - -');
if AREFLAG > 0
   [Y1,Y2,lamYc,Yerr,wellposed,Y] = aresolv(ay,qy,ry,aretype);
else
   [Y1,Y2,lamYc,Yerr,wellposed,Y] = aresolv(ay,qy,ry);
end
lamX = min(real(eig(X)));
lamY = min(real(eig(Y)));
format long
lamXY = max(real(eig(X*Y)))
if (lamX<0 & abs(lamX)>1e-10) | (lamY<0 & abs(lamY)>1e-10) | lamXY > 1
         disp('  ')
         disp('    ----------------------------------------------------');
         disp('       NO STABILIZING CONTROLLER MEETS THE SPEC. !!')
         disp('       ADJUST "Gam" AND TRY AGAIN ...')
         disp('    ----------------------------------------------------');
         break
end
%
% ---------------------------- Constant Matrices:
%
F   = - Ri*(d1d'*c1+B'*X);
F11 = F(1:m1-p2,:);
F12 = F(m1-p2+1:m1,:);
F2  = F(m1+1:m1+m2,:);
%
H   = - (b1*dd1'+Y*C')*Rwi;
H11 = H(:,1:p1-m2);
H12 = H(:,p1-m2+1:p1);
H2  = H(:,p1+1:p1+p2);
%
% ---------------------------- Controller in Descriptor form:
%
Z   = eye(n) - inv(gam2)*Y*X;
%
bk2    = (b2+H12)*dk12;
ck2    = -dk21*(c2+F12);
bk1    = -H2+bk2*inv(dk12)*dk11;
ck1    = F2+dk11*inv(dk21)*ck2;
ak     = A*Z + H*C*Z + bk2*inv(dk12)*ck1;
%
% ------------------------------------- Post-process the controller:
%
% ------------------------------------- 1) Reverse the controller scaling:
%
bk1 = bk1*sy;
ck1 = su*ck1;
dk11 = su*dk11*sy;
dk12 = su*dk12;
dk21 = dk21*sy;
%
% ------------------------------------- 2) Shifting D22 term:
%
temp = inv(eye(size(dk11)*[1;0]) + dk11*D22);
ak   = ak-bk1*D22*temp*ck1;
bk2  = bk2-bk1*D22*temp*dk12;
ck2  = ck2-dk21*D22*temp*ck1;
ck1  = temp*ck1;
dk22 = dk22-dk21*D22*temp*dk12;
dk12 = temp*dk12;
dk11 = temp*dk11;
temp = eye(size(D22)*[1;0])-D22*dk11;
bk1  = bk1*temp;
dk21 = dk21*temp;
dk22 = zeros(size(dk21)*[1;0],size(dk12)*[0;1]);
%
% ----------------  Putting descriptor controller K(s) in state space form:
%
[ak,bk,ck,dk] = des2ss(ak,[bk1,bk2],[ck1;ck2],...
                         [dk11,dk12;dk21,dk22],Z);
[rbk1,cbk1] = size(bk1);
[rbk2,cbk2] = size(bk2);
[rck1,cck1] = size(ck1);
[rck2,cck2] = size(ck2);
bk1=bk(:,1:cbk1);
bk2=bk(:,cbk1+1:cbk1+cbk2);
ck1=ck(1:rck1,:);
ck2=ck(rck1+1:rck1+rck2,:);
dk11=dk(1:rck1,1:cbk1);
dk12=dk(1:rck1,cbk1+1:cbk1+cbk2);
dk21=dk(rck1+1:rck1+rck2,1:cbk1);
dk22=dk(rck1+1:rck1+rck2,cbk1+1:cbk1+cbk2);
%
% --------------------- computing controller F(s) = (acp,bcp,ccp,dcp):
%
sysk = [ak,bk1,bk2;ck1,dk11,dk12;ck2,dk21,dk22];
dimk = [size(ak)*[1 0]',size(bk1)*[0 1]',size(bk2)*[0 1]',...
            size(ck1)*[1 0]',size(ck2)*[1 0]'];
if ~( exist('AU') & exist('BU') & exist('CU') & exist('DU'))
   acp = ak; bcp = bk1; ccp = ck1; dcp = dk11;
else
   [acp,bcp,ccp,dcp] = lftf(sysk,dimk,AU,BU,CU,DU);
end
%
% ---------------------------------------------------------------------------
%
% ------------------------------------ Closed-loop TF (Ty1u1):
%
sysp = [A B1 B2;C1 D11 D12;C2 D21 D22];
dimp = [n,size(B1)*[0 1]',size(B2)*[0 1]',...
       size(C1)*[1 0]',size(C2)*[1 0]'];
[acl,bcl,ccl,dcl] = lftf(sysp,dimp,acp,bcp,ccp,dcp);
%
clear sysp, clear dimp, clear a, clear b1, clear b2, clear c1, clear c2
clear d11, clear d12, clear d21, clear d22, clear syscp, clear dimcp
clear temp
%
% ---------- End of HINFKGJD.M --- RYC/MGS -- %