%
% HMATDEMO Fighter Design Demonstration
%

% R. Y. Chiang & M. G. Safonov 3/88
% Copyright (c) 1988 by the MathWorks, Inc.
% All Rights Reserved.
% ---------------------------------------------------------------
clear
clc
disp(' ')
disp('  ')
disp('  ')
disp('                << Demo #2: MIMO Fighter Design Example >>')
disp(' ')
disp(' ')
disp('                        ----')
disp('                         \  \          ----')
disp('                       == \ = \         \  \')
disp('                          /     \        \  \')
disp('                    / ---------------------------- \')
disp('                   |         FIGHTER OF 1990        >>>> -----')
disp('                    \ ---------------------------- /')
disp('                          \     /        /  /')
disp('                       == / = /         /  /')
disp('                         /  /          ----')
disp('                        ----')
format short e
ag =[
-2.2567e-02  -3.6617e+01  -1.8897e+01  -3.2090e+01   3.2509e+00  -7.6257e-01;
9.2572e-05  -1.8997e+00   9.8312e-01  -7.2562e-04  -1.7080e-01  -4.9652e-03;
1.2338e-02   1.1720e+01  -2.6316e+00   8.7582e-04  -3.1604e+01   2.2396e+01;
0            0   1.0000e+00            0            0            0;
0            0            0            0  -3.0000e+01            0;
0            0            0            0            0  -3.0000e+01];
bg = [0     0;
      0     0;
      0     0;
      0     0;
     30     0;
      0    30];
cg = [0     1     0     0     0     0;
      0     0     0     1     0     0];
dg = [0     0;
      0     0];
disp('  ')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
clc
disp('  ')
disp('   State-Space of the fighter pitch axis (trimed @ 25000 ft, 0.9 mach):')
ag 
bg
disp('                                 (strike a key to continue ...)')
pause
clc
cg
dg
disp('                                 (strike a key to continue ...)')
pause
clc
disp('  ')
disp('  ')
disp('    Poles of the Plant (unstable Phugoid modes):')
disp('  ')
disp('    ---------------------------------------------------------------')
disp('       poleg = eig(ag)      % Computing the poles of the plant')
disp('    ---------------------------------------------------------------')
poleg  = eig(ag)       
disp(' ')
disp(' ')
disp('                                 (strike a key to continue ...)')
pause
clc
disp(' ')
disp('  ')
disp('    Transmission Zeros of the Plant (minimum phase):')
disp('  ')
disp('    -------------------------------------------------------------------')
disp('      tzerog = tzero(ag,bg,cg,dg)   % Computing the transmission zeros')
disp('    -------------------------------------------------------------------')
tzerog = tzero(ag,bg,cg,dg)      % Computing the transmission zeros
disp('  ')
disp(' ')
disp(' ')
disp('                                 (strike a key to continue ...)')
pause
clc
disp(' ')
disp('      - - - Computing SV Bode plot of the open loop plant - - -')
w = logspace(-3,5,50);
svg = sigma(ag,bg,cg,dg,1,w); svg = 20*log10(svg);
disp(' ')
disp(' ')
disp(' ')
disp(' ')
disp('                (strike a key to see the SV Bode plot of G(s) ...)')
pause
clg
semilogx(w,svg)
title('MIMO Fighter Pitch Axis Open Loop')
xlabel('Frequency - Rad/Sec')
ylabel('SV - db')
grid
pause
clc
disp('  ')
disp('                    << Design Specifications >>  ')
disp(' ')
disp('      1). Robustness Spec. : -2 roll-off, -20 db @ 100 Rad/Sec.')
disp('          Associated Weighting:')
disp('     ')
disp('                    -1     1000')
disp('                  W3(s) = ------ * I       (fixd)')
disp('                            s^2     2x2')
disp('   ')
disp(' ')
disp('      2). Performance Spec.: minimizing the sensitivity function')
disp('                             as much as possible.')
disp('          Associated Weighting:')
disp(' ') 
disp('                    -1       -1   s + 0.01')
disp('                  W1(s) = Gam  * ----------- *  I')
disp('                                     1           2x2')
disp('   ')
disp('          where "Gam" in our design goes from 1 --> 8.4 --> 16.8.')
k = 1000; tau = 5.0000e-04; mn = [2 2];
nuw3i = [0 0 k]; dnw3i = [1 0 0]; 
svw3i = bode(nuw3i,dnw3i,w); svw3i = 20*log10(svw3i);
nuw1i = [1.0000e+00   1.0000e-02]; dnw1i =[0 1];
svw1i = bode(nuw1i,dnw1i,w); svw1i = 20*log10(svw1i);
disp('  ')
disp('  ')
disp('                   (strike a key to see the plot of the weightings ...)')
pause
semilogx(w,svw1i,w,svw3i)
grid
title('MIMO Fighter Design Example -- Design Specifications')
xlabel('Frequency - Rad/Sec')
ylabel('1/W1 & 1/W3 - db')
text(0.003,-70,'Sensitivity Spec.')
text(0.003,-100,'1/W1(s)')
text(200,-20,'Robustness Spec.')
text(1000,-50,'1/W3(s)')
pause
clc
disp('                      << Problem Formulation >>')
disp(' ')
disp('    Form an augmented plant P(s) with these two weighting functions:')
disp(' ')
disp('               1). W1 penalizing error signal "e"')
disp('  ')
disp('               2). W3 penalizing plant output "y"')
disp('  ')
disp('    and find a stabilizing controller F(s) such that the H2 or H-Inf norm')
disp('    of TF Ty1u1 is minimized and its H-Inf norm is less than one, i.e.')
disp('  ')
disp('                        min |Ty1u1|   < 1,')
disp('                        F(s)       inf')
disp(' ')
disp('      where ')
disp('                       |               -1|')                         
disp('               Ty1u1 = | Gam*W1*(I + GF) |  = | Gam * W1 * S  |')
disp('                       |               -1|    |  W3 * (I - S) |')
disp('                       |  W3*GF*(I + GF) |')
disp('  ')
disp('  ')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
clc
disp('  ')
disp(' ')
disp('                              << DESIGN PROCEDURE >>')
disp('  ')
disp('            * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *')
disp('            *    [Step 1]. Do plant augmentation (run AUGTF.M or      *')
disp('            *              AUGSS.M)                                   *')
disp('            *                                                         *')
disp('            *    [Step 2]. Do H2 synthesis (run H2LQG.M)              *')
disp('            *                                                         *')
disp('            *    [Step 3]. Redo the plant augmentation for a          *')
disp('            *              new "Gam" --> 8.4 and rerun H2LQG.M        *')
disp('            *                                                         *')
disp('            *    [Step 4]. Redo the plant augmentation for a          *')
disp('            *              higher "Gam" --> 16.8, then run HINF.M     *')
disp('            * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *')
disp('  ')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
clc
disp('  ')
disp('  ')
disp('           Assign the cost coefficient "Gam" --> 1 ')
disp('       ')
disp('           this will serve as the baseline design ....')
disp('  ')
disp('            ----------------------------------------------------------')
disp('             % Plant augmentation of the fighter:')
disp('             ss_g = mksys(ag,bg,cg,dg);')
disp('             w1 = [Gam*dnw1i;nuw1i;Gam*dnw1i;nuw1i];')
disp('             w2 = [];')
disp('             w3 = [0 1 0 0;0 0 0 k;tau 1 0 0;0 0 0 k];')
disp('             [TSS_] = augtf(ss_g,w1,w2,w3);')
disp('            ----------------------------------------------------------')
disp('   ')
disp(' ')
Gam = input('           Input the cost coefficient "Gam" = ');
ss_g = mksys(ag,bg,cg,dg);
w1 = [Gam*dnw1i;nuw1i;Gam*dnw1i;nuw1i];
w2 = [];
w3 = [0 1 0 0;0 0 0 k;tau 1 0 0;0 0 0 k];
[TSS_]=augtf(ss_g,w1,w2,w3);
disp(' ')
disp('     - - - State-Space TSS_:=(A,B1,B2,..,D21,D22) is ready for')
disp('           the Small-Gain problem - - -')
disp('  ')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
clc
disp(' ')
disp(' ')
disp('    --------------------------------------------------------------')
disp('     [ss_cp,ss_cl] = h2lqg(TSS_);  % Running H2LQG.M')
disp('     [acp,bcp,ccp,dcp] = branch(ss_cp);')
disp('     [acl,bcl,ccl,dcl] = branch(ss_cl);')
disp('    --------------------------------------------------------------')
[ss_cp,ss_cl] = h2lqg(TSS_);
[acp,bcp,ccp,dcp] = branch(ss_cp);
[acl,bcl,ccl,dcl] = branch(ss_cl);
disp('  ')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
pltopt           % Preparing singular values for plotting
svw1i1 = svw1i; h2svs1 = svs; h2svt1 = svt; h2svtt1 = svtt;
disp('  ')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
clc      
disp('  ')
disp('  ')
disp('    After a few iterations without changing the dynamics of the weights,')
disp(' ')
disp('    we found a new Gam of 8.4 can push the H2 cost function close to its')
disp('  ')
disp('    "shaping limit". ')
disp('  ')
disp('  ')
disp('            Input "Gam" --> 8.4, and try H2LQG again .....')
disp('  ')
disp('  ')
Gam = input('           Input the cost coefficient "Gam" = ');
w1 = [Gam*dnw1i;nuw1i;Gam*dnw1i;nuw1i];
[TSS_]=augtf(ss_g,w1,w2,w3);
disp(' ')
disp('     - - - New state-space TSS_ is ready for')
disp('           the Small-Gain problem - - -')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
clc
disp(' ')
disp(' ')
disp('    --------------------------------------------------------------')
disp('     [ss_cp,ss_cl] = h2lqg(TSS_);  % Running H2LQG.M')
disp('     [acp,bcp,ccp,dcp] = branch(ss_cp);')
disp('     [acl,bcl,ccl,dcl] = branch(ss_cl);')
disp('    --------------------------------------------------------------')
[ss_cp,ss_cl] = h2lqg(TSS_);
[acp,bcp,ccp,dcp] = branch(ss_cp);
[acl,bcl,ccl,dcl] = branch(ss_cl);
disp('  ')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
pltopt
svw1i2 = svw1i; h2svs2 = svs; h2svt2 = svt; h2svtt2 = svtt;
disp('  ')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
clc
disp('  ')
disp('  ')
disp('               Now let us try H-Inf synthesis .....')
disp('   ')
disp('    After a few iterations on the parameter "Gam", we found "Gam" can be')
disp(' ')
disp('    increased to 16.8, which is twice of the H2 approach !!  That is')
disp('   ')
disp('    using "Gam-iteration" one can squeeze more for a particular design')
disp(' ')
disp('    spec. than H2, hence, shape the singular values "exactly" to the pre-')
disp('  ')
disp('    specified frequency domain specifications.')
disp('  ')
disp('    Note that for H-Inf synthesis, the D11 matrix (i.e. W1(inf)) can')
disp('  ')
disp('    be NONZERO now (--> denominator of W1 = [0.01 1];).')
disp(' ')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
clc
dnw1i = [0.01 1];
disp('            ----------------------------------------------------------')
disp('             % Plant augmentation of the fighter:')
disp('             w1 = [Gam*dnw1i;nuw1i;Gam*dnw1i;nuw1i];')
disp('             [TSS_]=augtf(ss_g,w1,w2,w3);')
disp('            ----------------------------------------------------------')
disp('   ')
disp('             Input "Gam" --> 16.8, and try HINF.M ....')
disp(' ')
Gam = input('           Input the cost coefficient "Gam" = ');
w1 = [Gam*dnw1i;nuw1i;Gam*dnw1i;nuw1i];
[TSS_]=augtf(ss_g,w1,w2,w3);
disp(' ')
disp('     - - - State-Space TSS_ is ready for')
disp('           the Small-Gain problem - - -')
disp('  ')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
clc
disp(' ')
disp(' ')
disp('   --------------------------------------------------------------- ')
disp('    [ss_cp,ss_cl,hinfo] = hinf(TSS_);')
disp('    [acp,bcp,ccp,dcp] = branch(ss_cp);')
disp('    [acl,bcl,ccl,dcl] = branch(ss_cl);')
disp('                    % Running file HINF.M for H-Inf optimization')
disp('   ---------------------------------------------------------------')
[ss_cp,ss_cl,hinfo] = hinf(TSS_);
[acp,bcp,ccp,dcp] = branch(ss_cp);
[acl,bcl,ccl,dcl] = branch(ss_cl);
disp('  ')
disp('  ')
disp('                                 (strike a key to continue ...)')
pause
pltopt
svw1i3 = svw1i; hinfsvs = svs; hinfsvt = svt; hinfsvtt = svtt;
disp('  ')
disp('  ')
disp('                  (strike a key to see the plots of the comparison ...)')
pause
clg
semilogx(w,svw1i1,w,h2svs1,w,svw1i2,w,h2svs2,w,svw1i3,w,hinfsvs)
title('H2 & H-Inf Fighter Design -- 1/W1 & Sensitivity Func.')
xlabel('Frequency - Rad/Sec')
ylabel('SV - db')
grid
text(0.002,30,'H2 (Gam = 1) ---> H2 (Gam = 8.4) ---> H-Inf (Gam = 16.8)')
pause
semilogx(w,svw3i,w,h2svt1,w,h2svt2,w,hinfsvt)
title('H2 & H-Inf Fighter Design -- 1/W3 & Comp. Sens. Func.')
xlabel('Frequency - Rad/Sec')
ylabel('SV - db')
grid
text(0.005,100,'H2 (Gam = 1) ---> H2 (Gam = 8.4) ---> H-Inf (Gam = 16.8)')
pause
semilogx(w,h2svtt1,w,h2svtt2,w,hinfsvtt)
title('H2 & H-Inf Fighter Design -- Cost function Ty1u1')
xlabel('Frequency - Rad/Sec')
ylabel('SV - db')
grid
text(0.002,-25,'H2 (Gam = 1) ---> H2 (Gam = 8.4) ---> H-Inf (Gam = 16.8)')
pause
clc
disp('  ')
disp('  ')
disp('             << 8-State H-Inf Controller (Gam = 16.8) >>')
disp('    ')
disp('    Poles of the Controller :')
polecp = eig(acp)
disp(' ')
disp('                                 (strike a key to continue ...)')
pause
disp(' ')
disp('    Transmission Zeros of the Controller :')
tzcp = tzero(acp,bcp,ccp,dcp)
disp(' ')
disp('                                 (strike a key to continue ...)')
pause
svcp = sigma(acp,bcp,ccp,dcp,1,w); svcp = 20*log10(svcp);
semilogx(w,svcp)
title('8-State H-Inf Controller (Gam = 16.8)')
xlabel('Rad/Sec')
ylabel('SV (db)')
pause
clc
disp('    Poles of the Cost Function :')
poletyu = eig(acl)
disp('                                 (strike a key to continue ...)')
pause
%
% ------ End of HIMATDEMO.