%
% ---------------------------------------------------------------
%  MUDEMO.M is a script file that demonstrates the design of 
%     1990 ACC benchmark problem using MU-SYNTHESIS.
% ---------------------------------------------------------------
%
% R. Y. Chiang & M. G. Safonov 
% Copyright (c) 1988 by the MathWorks, Inc.
% All Rights Reserved.

clc
clear
disp(' ')
disp('                      << Benchmark Problem >>')
disp('  ');
disp('                           |--> x1        |--> x2 = z (measurement)');
disp('         (control)    ------     k   ------');
disp('           u -------> | M1 | -/\/\/\-| M2 | ------> w (disturbance)');
disp('                      ------         ------');
disp('                       0  0           0  0');
disp('   ===============================================================');
disp('                     [ Design Specifications ]');
disp('    ------------------------------------------------------------');
disp('    | Design | Robustness   | Settling Time | Disturbance w(t) |')
disp('    ---------+--------------+---------------+-------------------');
disp('    |   2    | maximize MSM | Ts ~= 15 sec  |     impulse      |');
disp('    |        | for nominal  | for nominal   |                  |');
disp('    ------------------------------------------------------------');
disp('   ')
disp('                                  (strike a key to continue ...)');
pause
disp(' ')
disp(' ')
spring = [1 0.5 2];
%
clc
disp(' ')
disp('  -----------------------------------------------------------')
disp('    1. Set up the nominal system:');
disp(' ');
disp('       k = 1; m1 = 1; m2 = 1;');
disp('       ag = [0      0     1     0');
disp('             0      0     0     1');
disp('          -k/m1   k/m1    0     0');
disp('           k/m2  -k/m2    0     0];');
disp('       bg = [0 0 1/m1 0]`; cg = [0 1 0 0]; dg = 0;');
disp('  -----------------------------------------------------------')
k = 1; m1 = 1; m2 = 1;
%
ag = [0      0     1     0
      0      0     0     1
   -k/m1   k/m1    0     0
    k/m2  -k/m2    0     0];
bg = [0 0 1/m1 0]';
cg = [0 1 0 0];
dg = 0;
disp(' ');
disp('       Poles of G(s) - -')
lamg = eig(ag)
disp(' ')
disp('                       (strike a key to continue)')
pause
%---------------------------------------
% Augment Plant for H-Inf Design:
%
clc
disp(' ')
   disp('  -----------------------------------------------------------');
   disp('    2. Set up the augment plant for H-Inf design:');
   disp(' ')
   disp('       Two-port state-space:')
   disp('          (A,BB1,BB2,CC1,CC2,DD11,DD12,DD21,DD22)')
   disp(' ')
w2flag = 0;
%   w2flag = input('     Select W2: Dynamic (1); Constant (2): ');
if w2flag ~= 0
   disp('          Port 1: 3 uncertainties + W2 ')
   disp('          Port 2: controller')
   disp('  -----------------------------------------------------------');
   disp(' ')
   rho = input('     Assign the DC of W2 weighting for control energy: ');
   if w2flag == 1
      nuw2 = rho*[1/10 1]; dnw2 = [1/200 1];
      [aw2,bw2,cw2,dw2] = tf2ss(nuw2,dnw2);
   else
      aw2 = []; bw2 = []; cw2 = []; dw2 = rho;
   end
   AA = diagmx(ag,aw2);
   BB1 = [ 0     0    0     0;
           0     0    0     0;
         -1/m1 -1/m1  0    k/m1;
          1/m2   0  -1/m2 -k/m2;
          zeros(size(aw2)*[1;0],4)];
   BB2 = [0;0;1/m1;0;bw2];
   CC1 =  [ 1    -1   0 0;
          -k/m1  k/m1 0 0;
           k/m2 -k/m2 0 0;
            0     0   0 0]; 
   CC1 = [CC1 [zeros(3,size(cw2)*[0;1]);cw2]];
   CC2 = [0 1 0 0 zeros(1,size(cw2)*[0;1])];
   DD11 = [0 0 0 0;-1/m1 -1/m1 0 k/m1;1/m2 0 -1/m2 0; 0 0 0 0];
   DD12 = [0;1/m1;0;dw2]; DD21 = [0 0 0 1]; DD22 = 0;
else
   disp('          Port 1: 3 uncertainties')
   disp('          Port 2: controller')
   disp('  -----------------------------------------------------------');
   AA = ag; BB1 = [0 0 0;0 0 0;-1/m1 -1/m1 0;1/m2 0 -1/m2]; 
   BB2 = [0 0 1/m1 0]'; 
   CC1 = [1 -1 0 0;-k/m1 k/m1 0 0;k/m2 -k/m2 0 0]; CC2 = [0 1 0 0];
   DD11 = [0 0 0;-1/m1 -1/m1 0;1/m2 0 -1/m2]; DD12 = [0 1/m1 0]'; 
   DD21 = [0 0 0]; DD22 = 0;
end   
%
disp(' ');
%
no_u1 = size(BB1)*[0;1]; no_u2 = size(BB2)*[0;1];
no_y1 = size(CC1)*[1;0]; no_y2 = size(CC2)*[1;0];
%
BB = [BB1 BB2]; CC = [CC1;CC2]; DD = [DD11 DD12;DD21 DD22];
%
disp(' ');
disp('       Poles of Augment Plant P(s) - -')
lamp = eig(AA)
disp('                (strike a key to continue)')
pause
% --------------------------------------
% JW-axis shifting from s -----> s~:
%
clg
bilexp
clc
disp(' ')
disp(' --------------------------------------------------------------------')
disp('    3. Transform the Augment Plant to Shifted JW-Axis:')
disp(' ')
disp('       % pack the 2-port state-space ...')
disp('       BB = [BB1 BB2]; CC = [CC1;CC2]; DD = [DD11 DD12;DD21 DD22];')
disp('       % select the circle point for mapping ...')
disp('       [aa,bb,cc,dd] = bilin(AA,BB,CC,DD,1,`Sft_jw`,cirpt);')
disp('       % split the (aa,bb,cc,dd) to 2-port state-space (A,B1,B2,..);')
disp(' --------------------------------------------------------------------') 
disp('      Input the circle point "p1" - - -')       
cirpt1 = input('     (-0.25: non-min. phase controller, -0.3: min. phase controller): ');
cirpt = [-100 cirpt1];
[aa,bb,cc,dd] = bilin(AA,BB,CC,DD,1,'Sft_jw',cirpt);
A = aa; B1 = bb(:,1:no_u1); B2 = bb(:,no_u1+1:no_u1+no_u2); 
C1 = cc(1:no_y1,:); C2 = cc(no_y1+1:no_y1+no_y2,:);
D11 = dd(1:no_y1,1:no_u1); D12 = dd(1:no_y1,no_u1+1:no_u1+no_u2); 
D21 = dd(no_y1+1:no_y1+no_y2,1:no_u1); 
D22 = dd(no_y1+1:no_y1+no_y2,no_u1+1:no_u1+no_u2);
disp(' ')
disp('       Poles of P(s~) - -')
lampp = eig(A)
disp(' ')
disp('                    (strike a key to continue)')
pause
clc
disp(' ')
disp('  ------------------------------------------------------------------')
disp('    4. Starting H-Inf Design in "s~" Domain:');
disp(' ')
disp('       TSS_ = mksys(A,B1,B2,C1,C2,D11,D12,D21,D22,`tss`);')
disp('       % H-Inf GAMMA-Iteration for maximum MSM')
disp('       [gamopt,ss_cp,ss_cl]=hinfopt(TSS_);')
disp('       [acp,bcp,ccp,dcp] = branch(ss_cp);')
disp('       [acl,bcl,ccl,dcl] = branch(ss_cl);')
disp('  ------------------------------------------------------------------')
disp('  ')
disp('  ')
disp('                    (strike a key to continue)')
pause
format short e
[gamopt,acp,bcp,ccp,dcp,acl,bcl,ccl,dcl]=hinfopt(...
                    A,B1,B2,C1,C2,D11,D12,D21,D22);
disp(' ')
disp(' ')
disp('                    (strike a key to continue)')
pause
[nucp,dncp] = ss2tf(acp,bcp,ccp,dcp,1);
w = logspace(-3,3,100);
[acp,bcp,ccp,dcp] = tf2ss(nucp,dncp);
% --------------------------------------------------------------------
% JW-axis shifting on H-Inf cost & controller from s~------>s
%
clc
disp(' ')
disp('   ----------------------------------------------------------------------')
disp('      Transform the cost & controller from "s~" to "s":')
disp(' ')
disp('      [acll,bcll,ccll,dcll] = bilin(acl,bcl,ccl,dcl,-1,`Sft_jw`,cirpt);')
disp('      [af,bf,cf,df] = bilin(acp,bcp,ccp,dcp,-1,`Sft_jw`,cirpt);')
disp('   ----------------------------------------------------------------------')
disp(' ')
disp(' ')
disp('                     (strike a key to continue)')
pause
[acll,bcll,ccll,dcll] = bilin(acl,bcl,ccl,dcl,-1,'Sft_jw',cirpt);
[af,bf,cf,df] = bilin(acp,bcp,ccp,dcp,-1,'Sft_jw',cirpt);   
disp(' ')
disp('     - - Computing the Cost Function Ty1u1(s~) and Ty1u1(s) - -');
[perttw1,diag_d] = ssv(acl,bcl,ccl,dcl,w);
msmw1 = gamopt/max(perttw1)*100;
perttw1 = 20*log10(perttw1);
[pertt1,diag_dl]  = ssv(acll,bcll,ccll,dcll,w);
msm1 = gamopt/max(pertt1)*100;
pertt1  = 20*log10(pertt1);  
clg
semilogx(w,[perttw1;pertt1]);
title('Cost Function in "s~" and "s"');
grid;xlabel('Rad/Sec'); ylabel('PERRON SSV (db)');
text(0.2,0.3,['ADDITIVE MSM IN s~-DOMAIN: +-',num2str(msmw1),' %'],'sc');
text(0.2,0.2,['ADDITIVE MSM IN s-DOMAIN : +-',num2str(msm1),' %'],'sc');
%prtsc
disp(' ')
disp(' ')
disp('                     (strike a key to continue)')
pause
clc
disp(' ')
disp('  ---------------------------------------------------------------')
disp('    5. Start to Curve-Fit the Diagonal Scaling D Matrix:')
disp('       (compute the cost function (acl,bcl,ccl,dcl) again)')
disp('    ')
disp('       w = logspace(-3,3,100);')
disp('       [SS_D] = fitd(diag_d,w,[3 2 2]);')
disp('     % Absorb the D(s) and inv(D(s)) into the augmented plant:')
disp('       [TSS_D] = augd(TSS_,SS_D);')
disp('  ----------------------------------------------------------------')
disp(' ')
disp(' ')
disp('                     (strike a key to continue)')
pause
%
if w2flag == 0
   ord_d = [3 2 2];
else
   ord_d = [2  2 2 3];
end
[AD,BD,CD,DD] = fitd(diag_d,w,ord_d);
[ga1,ph1] = bode(AD,BD(:,1),CD(1,:),DD(1,1),1,w);
[ga2,ph2] = bode(AD,BD(:,2),CD(2,:),DD(2,2),1,w);
[ga3,ph3] = bode(AD,BD(:,3),CD(3,:),DD(3,3),1,w);
if w2flag ~= 0
   [ga4,ph4] = bode(AD,BD(:,4),CD(4,:),DD(4,4));
end
SS_D = mksys(AD,BD,CD,DD);
TSS_ = mksys(A,B1,B2,C1,C2,D11,D12,D21,D22,'tss');
TSS_D = augd(TSS_,SS_D);
clc
disp(' ')
disp('  -------------------------------------------------------------------')
disp('    6. Design the H-Inf Controller for the Diagonally-Scaled Plant')
disp(' ')
disp('       [gamopt,ss_cp,ss_cl] = hinfopt(TSS_D);')
disp('       [acp,bcp,ccp,dcp] = branch(ss_cp);')
disp('       [acl,bcl,ccl,dcl] = branch(ss_cl);')
disp('  -------------------------------------------------------------------')
disp(' ')
disp(' ')
disp('                    (strike a key to continue)')
pause
[gamopt,ss_cp,ss_cl] = hinfopt(TSS_D);
[acp,bcp,ccp,dcp] = branch(ss_cp);
[acl,bcl,ccl,dcl] = branch(ss_cl);
disp(' ')
disp(' ')
disp('                    (strike a key to continue)')
pause
%
mueva
muplt
%
% ------------ End of MUDEMO.M % RYC/MGS %






