% DINTDEMO.M --- A demo of hinf design for a double integrator plant
%

% R. Y. Chiang & M. G. Safonov 
% Copyright (c) 1988 by the MathWorks, Inc.
% All Rights Reserved.

clc
disp(' ')
disp(' ----------------------------------------------------------------')
disp('         << H-INF DESIGN FOR A DOUBLE INTEGRATOR PLANT >>')
disp(' ')
disp('   Model: a double integrator with moment inertia J (1/J/s/s)')
disp(' ')
disp('   For example: rigid body dynamics of a spacecraft (J = 5700)')
disp(' ')
disp('                           * * * * *')
disp('      * * * * * * * *      * space *      * * * * * * * *')
disp('      * solar panel * ---- * craft * ---- * solar panel *')
disp('      * * * * * * * *      *1/j/s/s*      * * * * * * * *')
disp('                           * * * * *')
disp('          ')
disp('   Objective: Find a stabilizing Mixed-Sensitivity H-Inf')
disp('              controller.')
disp(' ----------------------------------------------------------------')
disp('   ')
disp('                                  (strike a key to continue ...)');
pause
den = conv([1 0],[1 0]);
[ag,bg,cg,dg] = tf2ss(1/5700,den);
clc
disp(' ' )
disp(' ----------------------------------------------------------------')
disp('                 << JW-AXIS POLES/ZEROS >>')
disp(' ')
disp('  One special feature of state-space MIXED-SENSITIVITY approach:')
disp('   ')
disp('     PLANT DOES NOT ALLOWED TO HAVE JW-AXIS POLE/ZERO !!')
disp('   ')
disp('  But this does not mean we can not deal with the situation.')
disp('  Simply use the jw-axis shifting technique to shift the poles:')
disp(' ')
disp('    Step 1:  Shift the plant (Ag <----- Ag + SIGMA * eye(Ag))')
disp('    Step 2:  Do a standard mixed-sensitivity H-Inf design')
disp('    Step 3:  Shift back the controller ')
disp('             (Acp <------ Acp - SIGMA * eye(Acp))')
disp('                       Done !!')
disp('  where SIGMA can be a small positive number (e.g., 0.1)')
disp(' ---------------------------------------------------------------')
disp('   ')
disp('                                  (strike a key to continue ...)');
pause
SIGMA = 0.1;
ag0 = ag + SIGMA*eye(2);
disp('   ')
disp(' Poles of the plant/shifted plant:')
[eig(ag) eig(ag0)]
disp('   ')
disp('                                  (strike a key to continue ...)');
pause
clc
disp(' ')
disp(' --------------------------------------------------------------------')
disp('               << W3 WEIGHTING & JW-AXIS ZEROS >>')
disp('')
disp('  We have delt with the jw-axis plant POLES by shifting the axis.')
disp('  But there are still two plant ZEROS at infinity, which are also on ')
disp('  the jw-axis ....')
disp('  We can solve this problem via a clever W3 weighting:')
disp('  ')
disp('                        W3 = s^2 / 100')
disp('  ')
disp('  where the double differentiator makes the plant full rank at  ')
disp('  infinity, but also serves as the compelmentary sensitivity')
disp('  wighting function to control the system bandwidth.')
disp('  In our example, system bandwidth is set to be 10 rad/sec.')
disp(' ---------------------------------------------------------------------')
disp('   ')
disp('                                  (strike a key to continue ...)');
pause
clc
disp(' ')
disp(' ---------------------------------------------------------------------')
disp('       << SENSITIVITY WEIGHTING (W1), THE H-INF DESIGN KNOB >>')
disp(' ')
disp('  By tuning W1 filter, we can find an H-Inf controller:')
disp('                          2                                2')
disp('            beta * [alfa*S + 2*zeta1*w1c*sqrt(alfa)*S + w1c ]')
disp('      W1 = ----------------------------------------------------')
disp('                          2                                2')
disp('                   [beta*S + 2*zeta2*w1c*sqrt(beta)*S + w1c ]')
disp('   ')
disp('  where ')
disp('      beta: DC gain of the filter (controls the disturbance rejection)')
disp('      alfa: high frequency gain (controls the peak overshoot)')
disp('      w1c: filter cross-over frequency')
disp('      zeta1, zeta2: damping ratios of the corner frequencies.')
disp(' ')
disp('  Inverse of W1 is the desired shape of the sensitivity function.')
disp('  Here, we choose 2nd order W1 filter for a uniform loop-shaping on ')
disp('  L(s) = G(s)*F(s).')
disp(' --------------------------------------------------------------------')
disp('   ')
disp('                                  (strike a key to continue ...)');
pause
w1c = input('Input the sensitivity cross-over frequqncy (try 3): ');
beta = 100;
alfa = input('Input the sensitivity upper bound "ALFA" (try 1.5): ');
alfa = 1/alfa;
zeta1 = 0.7; zeta2 = 0.7;
w1 = [beta*[alfa 2*zeta1*w1c*sqrt(alfa) w1c*w1c];...
           [beta 2*zeta2*w1c*sqrt(beta) w1c*w1c]];
w2 = [];
w3 = [1 0 0;0 0 100];
clc
disp(' -----------------------------------------------------------')
disp('                   << H-INF DESIGN >>')
disp(' ')
disp('   >> w1 = [beta*[alfa 2*zeta1*w1c*sqrt(alfa) w1c*w1c];..')
disp('                 [beta 2*zeta2*w1c*sqrt(beta) w1c*w1c]];')
disp('   >> w2 = [];')
disp('   >> w3 = [1 0 0;0 0 100];')
disp('   >> ss_g = mksys(ag0,bg,cg,dg);')
disp('   >> TSS_ = augtf(ss_g,w1,w2,w3);')
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(' -----------------------------------------------------------')
disp('   ')
disp('                                  (strike a key to continue ...)');
pause
disp(' ')
disp(' - - - Working Plant Augmentation - - - Wait - - -')
ss_g = mksys(ag0,bg,cg,dg);
TSS_ = augtf(ss_g,w1,w2,w3); 
clc
[ss_cp,ss_cl,hinfo] = hinf(TSS_);
[acp,bcp,ccp,dcp] = branch(ss_cp);
[acl,bcl,ccl,dcl] = branch(ss_cl);
acp = acp-SIGMA*eye(acp);
if max(real(eig(acl))) < 0
  dinteva
  disp(' ')
  dintplt
end
%
% ---------- End of DINTDEMO.M % RYC/MGS
