 echo on
%
%	himat_x1
%
%  This MATLAB script file is an example of how to design H-infinity 
%   and mu-synthesis control laws, as applied to pitch axis control 
%   for HIMAT. The purpose of this MATLAB script is to familiarize the
%   user with the MATLAB function files developed for control design
%   in this toolbox.
%
%   These control designs will be compared with a loop-shaping 
%   control design using singular values, mu-analysis tools, the
%   output sensitivity, input complementary sensitivity
%   transfer functions, and time domain responses to step functions.
%   
%
%   The files are set up as follows:

 pause    % strike any key to continue
%		                                   
%  	                                  	  
%   himat_x1	setup file for H-infinity control design, first 
%               mu-synthesis iteration.
%   himat_x2	second iteration of mu-synthesis using d-scales 
%               from himat_x1.
%   himat_x3	third iteration of mu-synthesis using d-scales 
%               from himat_x2.
%   himat_x4 	fourth and final iteration of mu-synthesis
%   himat_x5	comparing mu-synthesis control law with loop-shaping 
%               design using singular values and mu.
%   himat_x6	time domain analysis of both control laws.
 
 pause   % strike any key to continue
clc

% First load the state space description of the HIMAT plane.
%  This matrix is described in the manual. The states of the
%  plant model are: forward speed (v), angle-of-attack (alpha),
%  pitch rate (q) and pitch angle (theta). The inputs 
%  are the elevon position and the canard position. The outputs 
%  that are to be kept small are angle-of-attack (alpha) and pitch 
%  angle (theta).
 
 pause   % strike any key to continue
 mkhimat;
 minfo(himat)
 
%    Next construct the weights associated with the multiplicative
%      input uncertainty, using the command ND2SYS
% 
%                             50(s+100)
%                   wdel  =   ---------
%                             (s+10000)

wdel = nd2sys([50,5000],[1,10000]);

pause % strike any key to continue
clc

%
%    and the output error weight (performance),
% 
%	   	  .5*(s+3)
% 	  wp  =   --------
%		   (s+.03)

wp = nd2sys([0.5,1.5],[1,0.03]);

pause % strike any key to continue
clc
 
%  Do a frequency_response of each weight and plot them on
%   the same graph for comparison purposes. Evaluate each transfer
%   function from 1e-4 to 1e+4 for a total of 40 points.
%   On the graph the solid line is the uncertainty weight and
%   the dashed line is the performance weight.
 
 om1 = logspace(-3,3,40);     % frequency vector
 wdelg = frsp(wdel,om1);      % frsp of disturbance weight
 wpg = frsp(wp,om1);          % frsp of performance weight
 wghts = sbs(wdelg,wpg);      % place responses "side-by-side"
 clg;
 vplot('liv,lm',wghts);
 echo off;
 title('Uncertainty Weight and Performance Weight for HIMAT');
 xlabel('Frequency (rad/s)');
 ylabel('Magnitude');
 pause   % strike any key to continue
 clear wdelg wpg wghts
 echo on;
 pause   % strike any key to continue
 
%  Construct a two input/two-output weight for both the 
%   uncertainty weight, WDEL and the output error weight,
%   WP. These weights are associated with each channel 
%   of the multivariable system. Each weight has 2 states, 
%   2 inputs and 2 outputs as one can see using MINFO.
 
 wdel = daug(wdel,wdel);
 wp = daug(wp,wp);
 minfo(wdel)
 minfo(wp)
 pause   % strike any key to continue
 
%  Now that the plant and the weights have been defined, 
%   construct the interconnection structure, HIMAT_IC, 
%   based on the diagram provided. A MATLAB function file, 
%   HIMATIC, has been written to do this for you.
%   SYSIC is used to formulate the interconnection structure.
%   HIMAT_IC has 8 states, 6 inputs and 6 outputs. The 
%   first two inputs and outputs [1 2] correspond to the 
%   multiplicative input uncertainty block, 
%   the second two inputs and outputs [3 4] correspond to
%   the disturbance inputs and error outputs. Inputs and 
%   outputs [5 6] are the measurements and controls being 
%   sent to and received from the controller.
 
 himatic
 minfo(himat_ic)
 clear wp wdel 
 pause   % strike any key to continue
 
% The next step is to design a H-infinity control law for HIMAT.
%  The function HINF designs a H-infinity control law based
%  on the interconnection structure provided. HINF requires 
%  the system interconnection structure (p), number of 
%  measurements (nmeas), number of controls (ncon), a lower bound on 
%  gamma (gmin), an upper bound on gamma (gmax), a tolerance for 
%  the  gamma iteration (tol)  and which method to use in solving 
%  the Riccati equations (ricmethd). Optional input arguments are the
%  epsilon for Hamiltionian jw-axis eigenvalues (epr) and the epsilon
%  for the Riccati solution positive definite tests (epp).  The two 
%  methods are via eigenvalue or Schur  decomposition. HINF returns 
%  the control law, k, the closed loop system, g, and the gamma 
%  value achieved, gf.
%
%      [k,g,gf]=hinfsyn(p,nmeas,ncon,gmin,gmax,tol,ricmethd,epr,epp)
 
 pause   % strike any key to continue
 
% In this example, the system interconnection structure is 
%  HIMAT_IC, with 2 measurements, 2 controls, a gamma low of 0.8, 
%  gamma upper of 6, a tolerance on the gamma iteration of .1, 
%  and we'll use the Schur decomposition method (2) to solve the 
%  Riccati equations. The default values of epr (1e-10) and epp (1e-6)
%  will be used for the epsilon tests.
 
 [k1,g1,gf1]=hinfsyn(himat_ic,2,2,0.8,6,0.05,2);
 
% A H-infinity control law has been designed which achieves an
%  infinity norm of 1.6938 for the interconnection structure
%  provided. First, we will examine aspects of the controller
%  that was just designed, starting with the controller POLES.

rifd(spoles(k1))
pause % strike any key to continue 
clc

%  Next, a magnitude plot of the frequency response of k1

om2=logspace(0,4,16);
k1_g = frsp(k1,om2);
vplot('liv,lm',k1_g)
echo off
title('Magnitude plot of controller, k1')
xlabel('strike any key to continue')
pause
echo on
pause   % strike any key to continue
format short e
clc

%  Onto the closed loop, first checking that it is stable.
%   Use the command SPOLES and RIFD.

 rifd(spoles(g1))
 pause   % strike any key to continue

%  Now calculate a closed loop frequency response. This results in
%  a 4x4 varying matrix G1G

 g1g=frsp(g1,om2);

%  Next, find the peak norm of the frequency response, using
%   PKVNORM. This corresponds to the maximum singular of the
%   closed loop system.

 g1pk=pkvnorm(g1g);
 echo off
 fprintf( '\n final gamma %g, \n max singular value %g \n \n',gf1,g1pk)
 echo on
 g1gs=vsvd(g1g);
 vplot('liv,m',g1gs)
 echo off
 title('Singular value plot of the H_infinity closed loop');
 xlabel('Frequency (rad/s)');
 ylabel('Magnitude');
 grid;
 pause
 echo on
 pause   % strike any key to continue
 
% The H-infinity control law can be analyzed using mu-analysis.
%  The closed-loop system, G1, has 4 inputs and 4 outputs. 
%  The first two inputs and outputs correspond to the 
%  uncertainty block, and the second two correspond to the 
%  disturbance rejection block, or performance block.
%  Therefore, we can define the uncertainty inputs and outputs
%  as a full 2x2 uncertainty block and the disturbance 
%  rejection inputs and outputs as a full 2x2 performance 
%  block. If the mu value for this control design is 1, then
%  we are able to achieve robust performance 
%  for the set of defined weights and this control design.

 pause   % strike any key to continue
 
%   mu  -  usage: [bnds,row_d,sens,row_p] = mu(matin,blk,'opt')
 
%  The mu-analysis program, MU, requires a frequency response
%  or constant matrix and a block structure. In this example,
%  the frequency response is G1G and the block structure is
%  two, 2x2 full blocks. MU returns the upper and lower 
%  bounds for mu in BNDS1, the frequency
%  varying d-scales DV1, the rational perturbation which 
%  destabilizes the closed-loop plant RP1 and the 
%  sensitivity of mu to the d-scales SENS1.
 
 blk=[2 2;2 2];
 [bnds1,dv1,sens1,rp1]=mu(g1g,blk);

% Plot the maximum singular value and mu on the same plot
%   solid line - maximum singular value
%   dashed line - mu
 
 both=sbs(sel(g1gs,1,1),bnds1);
 vplot('liv,m',both)
 echo off
 title('Max. singular value and mu for the  H-infinity Control Law');
 xlabel('Frequency (rad/s)');
 ylabel('Magnitude');
 grid;
 pause   % strike any key to continue
 echo on

% clear variables not used and pack the memory space 
%  due to the limitations on the Mac and PC's.
 
 clear g1g g1gs both 
 pack
 
% This concludes the H-infinity control design portion of this
%   demonstration program. If all you wanted was the H-infinity 
%   control law, you can stop here just type in the number 1.
%   Otherwise, we'll precede with the first iteration of 
%   mu-synthesis.
 
echo off
 fini = input(' Enter the number 1 to stop this program ');
 if fini == 1
   return
 end
echo on
 
% Designing an H-infinity control law is the first step in 
%   mu-synthesis. The next step is to fit the d-scales which
%   are output from the MU program. The MUSYNFIT function
%   is designed to fit the d-scales for each individual block. 
%   If there are n full blocks in the mu-problem, n-1 d-scales 
%   are fit, since the d-scales are normalized to make 
%   the last one 1.
 
 pause   % strike any key to continue

 musynfit 
 
%  MUSYNFIT requires the left d-scale from the previous 
%   mu-synthesis iteration (PRE_DSYSL), the d-scales from the 
%   mu-analysis problem (DDATA), and the sensitivity of 
%   the d-scales (SENS), along with the block
%   structure (BLK), number of controller inputs (NMEAS) 
%   and outputs (NCNTRL). Since this is the first mu-synthesis 
%   iteration, the left d-scale is input as 'first'.
%   This creates an identity matrix of the correct size for dsysl.
%   Output from MUSYNFIT is the left d-scale (DSYSL) and  
%   right d-scale (DSYSR) to be multiplied into the 
%   interconnection structure.
 
 pause    % strike any key to continue
 
%   The previous d-scale is required since if new d-scales
%   are continuously generated of a given order, the number
%   of states in the interconnection structure will grow 
%   dramatically with each iteration. Therefore, the
%   previous d-scale is normalized output of the new 
%   d-scale data, and the new d-scale is wrapped around
%   the original interconnection structure.
%   For the first iteration, there are no d-scales in the problem.
 
%   We recommend a a 3rd order transfer function. This increases 
%   the number of states in the interconnection structure to
%   3*(size of full block)*2. If one selects a different order
%   for the d-scale weight, the gamma iteration will converge
%   to a different value.
 
 [dsysl,dsysr]=musynfit('first',dv1,sens1,blk,2,2);
 
% Now, wrap the new d-scales around the original 
%  interconnection structure.
%
%          himat_ic2 = dsysl*himat_ic*(dsysr)^-1
 
 pause    % strike any key to continue
 himat_ic2=mmult(dsysl,mmult(himat_ic,minv(dsysr)));

 minfo(himat_ic2)
echo off
 clear a b c dsysr fini g1 g1pk
 pack;
echo on
 
% Thus ends the first iteration of mu-synthesis. Notice that 
%   the new interconnection structure has 20 states, 12 more 
%   states than the original interconnection structure. These 
%   12  states are due the frequency varying d-scales which 
%   were fit. (Note, this is if you fit the d-scales using a 
%   3rd order system.)
 
%  Type in 'himat_x2' to continue the mu-synthesis iteration
%    procedure.
%
% Copyright MUSYN INC 1991,  All Rights Reserved
