%
%     << 4-block L-inf optimal Controller (all solution formulae) >>
%           (derived by Limebeer and Kasenally, Dec. 1987)
%
%   HINFLIM is a script file which computes the H-inf optimal controller 
%   using L-K'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 (c) 1988 by the MathWorks, Inc.
% 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 Limebeer and Kasenally) - - -')
%
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 the D22 term:
%
[mD22,nD22] = size(D22);
d22 = zeros(mD22,nD22);
%
% ------------------------------------------- 2) Find scaling factors:
%
[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;
c1 = sz*C1;
b2 = B2*su;
c2 = sy*C2;  
d11 = sz*D11*sw;
d12 = sz*D12*su;
d21 = sy*D21*sw;
%
% ----------------------------------- 3) Checking the frequency @ inf:
%
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
%
% ----------------------------------- 4) Orthogonal complements of D12 & D21:
%
d12p = ortc(d12);
d21p = ortr(d21);
% 
% ----------------------------------------------------------------------------
% 
% ---------------------------------------- Start to compute the controller:
siz = [1 0]';
n = size(A)*siz;
gam = 1;                    % for small-gain problem
gam2 = gam*gam;
RI = gam2*eye(n);
%
% ---------------------------------------- Fix D11 term:
%
if m1 == p2 | p1 == m2
   fopt = -d1122;
else
   fopt  = -(d1122+d1121*inv(eye(m1-p2)-d1111'*d1111)*d1111'*d1112);
end
%
a   = A  + b2*fopt*c2;
b1  = b1 + b2*fopt*d21;
c1  = c1 + d12*fopt*c2;
d11 = d11+ d12*fopt*d21;
%                                                  ~
% ---------------------------------------- Solving P Riccati:
%
ar1 = a+(b1*d11'*d12p*d12p' - b2*d12'*gam2)*...
      inv(gam2*eye(size(d11)*siz)-d11*d11'*d12p*d12p')*c1;
q1 = c1'*d12p*inv(gam2*eye(size(d12p')*siz)-...
     d12p'*d11*d11'*d12p)*d12p'*c1;
r1 = RI*b2*b2'-(b1-b2*d12'*d11)*gam2*inv(gam2*eye(size(d11')*siz)-...
     d11'*d12p*d12p'*d11)*(b1'-d11'*d12*b2');
disp(' ');
disp(' - - - Solving the 1st Riccati - - -');
if AREFLAG > 0
   [P1,P2,lamPc,Perr,wellposed,P] = aresolv(ar1,q1,r1,aretype);
else
   [P1,P2,lamPc,Perr,wellposed,P] = aresolv(ar1,q1,r1);
end
%                                                  ~
% ---------------------------------------- Solving S Riccati:
%
ar2 = a+b1*inv(gam2*eye(size(d21p')*siz)-d21p'*d21p*d11'*d11)*...
      (d21p'*d21p*d11'*c1-gam2*d21'*c2);
q2 = b1*d21p'*inv(gam2*eye(size(d21p)*siz)-...
     d21p*d11'*d11*d21p')*d21p*b1';
r2 = RI*c2'*c2-(c1'-c2'*d21*d11')*gam2*inv(gam2*eye(size(d11)*siz)-...
     d11*d21p'*d21p*d11')*(c1-d11*d21'*c2);
disp('   ');
disp(' - - - Solving the 2nd Riccati - - -');
if AREFLAG > 0
   [S1,S2,lamSc,Serr,wellposed,S] = aresolv(ar2',q2,r2,aretype);
else
   [S1,S2,lamSc,Serr,wellposed,S] = aresolv(ar2',q2,r2);
end
lamP  = min(real(eig(P)));
lamS  = min(real(eig(S)));
format long
lamPS = max(real(eig(P*S)))
if (lamP<0 & abs(lamP)>1e-10) | (lamS<0 & abs(lamS)>1e-10) | lamPS > 1
         disp('  ')
         disp('    ----------------------------------------------------');
         disp('       NO STABILIZING CONTROLLER MEETS THE SPEC. !!')
         disp('       ADJUST "Gam" AND TRY AGAIN ...')
         disp('    ----------------------------------------------------');
         break
end
%
% ---------------------------- Controller in descriptor form:
%
ek = eye(n)-S*P*RI;
bk1 = -gam2*(b1+S*c1'*d11 -S*c2'*d21*...
      (d11'*d11-gam2*eye(size(d11')*siz)))...
      *inv(gam2*eye(size(d21p')*siz)-...
      d21p'*d21p*d11'*d11)*d21';
ck1 = -gam2*d12'*inv(gam2*eye(size(d11)*siz)...
      -d11*d11'*d12p*d12p')...
      *(c1+d11*b1'*P-(d11*d11'-...
      gam2*eye(size(d11)*siz))*d12*b2'*P);
dk11 = zeros(size(ck1)*siz,size(bk1)*[0 1]');
%
bk2 = b1*d21'*d22-b2+(gam2*S*c1'+b1*d21p'*d21p*d11'-...
         gam2*S*c2'*d21*d11')*...
         inv(gam2*eye(size(d11)*siz)-...
         d11*d21p'*d21p*d11')*(d11*d21'*d22-d12)+...
         gam2*S*c2'*d22;
rbk2 = chol(d12'*(gam2*eye(size(d11)*siz)-d11*d11')...
         *gam2*inv(gam2*eye(size(d12p)*siz)-...
         d12p*d12p'*d11*d11')*d12);
rbk2 = rbk2';
bk2 = bk2*rbk2;
ck2 = (d22*d12'*c1-c2)+d22*b2'*P*gam2+(d22*d12'*d11-d21)...
         *inv(gam2*eye(size(d11')*siz)-d11'*d12p*d12p'*d11)*...
         (d11'*d12p*d12p'*c1-d11'*d12*b2'*P*gam2+b1'*P*gam2);
rck2 = chol(gam2*d21*inv(gam2*eye(size(d11')*siz)-...
         d11'*d11*d21p'*d21p)*...
         (gam2*eye(size(d11')*siz)-d11'*d11)*d21');
ck2 = rck2*ck2;
dk12 = chol(d12'*(gam2*eye(size(d11)*siz)-d11*d11')*...
           gam2*inv(gam2*eye(size(d12p)*siz)...
          -d12p*d12p'*d11*d11')*d12);
dk12 = -dk12';
dk21 = chol(gam2*d21*inv(gam2*eye(size(d11')*siz)-...
            d11'*d11*d21p'*d21p)*...
           (gam2*eye(size(d11')*siz)-d11'*d11)*d21');
dk21 = -dk21;
dk22 = dk21*d22*dk12+inv(dk21')*d21*d11'*...
          inv(gam2*eye(size(d11)*siz)-...
          d11*d21p'*d21p*d11')*d12*dk12*gam2;
%
ak = -(S*c1'*d11+b1)*inv(gam2*eye(size(d21p')*siz)-...
      d21p'*d21p*d11'*d11)...
     *d21'*d21*inv(gam2*eye(size(d11')*siz)-...
     d11'*d12p*d12p'*d11);
ak = ak*(d11'*d12p*d12p'*c1*gam2+gam2*gam2*b1'*P-...
     gam2*gam2*d11'*d12*b2'*P...
     +gam2*(gam2*eye(size(d11')*siz)-...
     d11'*d12p*d12p'*d11)*d21'*c2);
ak0 = -S*c2'*d21*inv(gam2*eye(size(d11')*siz)-...
      d11'*d11*d21p'*d21p)*...
      (gam2*eye(size(d11')*siz)-d11'*d11)*...
      inv(gam2*eye(size(d11')*siz)-d11'*d12p*d12p'*d11);
ak0 = ak0*(d11'*d12p*d12p'*c1*gam2+gam2*gam2*b1'*P-...
       gam2*gam2*d11'*d12*b2'*P +...
       gam2*(gam2*eye(size(d11')*siz)-...
       d11'*d12p*d12p'*d11)*d21'*c2);
ak = ak+ak0;
ak = ak+ek*(a+(b1*d11'*d12p*d12p'-b2*d12'*gam2)*...
     inv(gam2*eye(size(d11)*siz)-...
     d11*d11'*d12p*d12p')*c1-(gam2*b2*b2'-...
     (b1-b2*d12'*d11)*gam2*inv(gam2*eye(size(d11')*siz)-...
     d11'*d12p*d12p'*d11)*(b1'-d11'*d12*b2'))*P);
ak = ak+bk1*d22*ck1;
%
% --------------- Flip the sign of controller:
%
bk1 = -bk1; bk2 = -bk2;
dk11 = -dk11; dk12 = -dk12; dk21 = -dk21; dk22 = -dk22;
%
% ------------------------------------ Post-process the controller:
%
% ------------------------------------ 1) Reverse the "fopt" term:
%
dk11 = dk11 + fopt;
%
% ------------------------------------ 2) Reverse the controller scaling:
%
bk1  = bk1*sy;
ck1  = su*ck1;
dk11 = su*dk11*sy;
dk12 = su*dk12;
dk21 = dk21*sy;
%
% ------------------------------------ 3) 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;
%
% ----------------  Putting descriptor controller K(s) in state space form:
%
se = svd(ek);
nullE = size(find(se<1.e-7))*[1;0];
[ak,bk,ck,dk] = des2ss(ak,[bk1,bk2],[ck1;ck2],...
                         [dk11,dk12;dk21,dk22],ek,nullE);
[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 HINFLIM.M --- RYC/MGS -- %
