%  Example 10.6.  mu-synthesis and LQG/LTR missile autopilot design.
%  This example is performed using six Matlab m-files:
%    Ex10_6.m    - Generates the mu-synthesis autopilot.
%    Ex10_6P.m   - Generate a step response and a Nyquist plot for the 
%                  mu-synthesis autopilot.  Must be executed after "Ex10_6.m"
%    Ex10_6RP.m  - Generate robust performance for the mu-synthesis autopilot.
%                  Must be executed after "Ex10_6.m"
%    Ex10_6LQG.m - Generates the LQG/LTR autopilot.
%    Ex10_6LP.m  - Generate a step response and a Nyquist plot for the 
%                  LQG/LTR autopilot.  Must be executed after "Ex10_6LQG.m"
%    Ex10_6LRP.m - Generate robust performance for the LQG/LTR autopilot.
%                  Must be executed after "Ex10_6LQG.m"
%
%  Note:  The first three programs should be performed in sequence, and the 
%    second three programs should be performed in sequence.
clear
%  Define the aerodynamics model.
%    States: alpha, theta_dot
%    Inputs: delta, wZa, wMa, wMtd
%    Outputs:  eta, alpha, d*theta_2dot, theta_dot
Za=-5.24;Zd=-0.4;Ma=-47;Mtd=-4.69;Md=-1134;V=1.82;d=8.55e-4;
Aa=[Za  1
    Ma Mtd];
Ba=[Zd 1 0 0
    Md 0 1 1];
Ca=[V*Za   0
     1     0
    Ma*d Mtd*d
     0     1  ];
Da=[V*Zd V 0 0
      0  0 0 0
    Md*d 0 d d
      0  0 0 0];
Aero=pck(Aa,Ba,Ca,Da);
%  Define the servo model.
wn=189;z=0.7;
As=[   0      1
    -wn*wn -2*z*wn];
Bs=[  0
    wn*wn];
Cs=eye(2);
Ds=[0 0]';
Servo=pck(As,Bs,Cs,Ds);
%  Define flexible body dynamics weight.
nufb=1.75e-4*[1 1]'*conv([1/0.0025 1],[1/25 1]);
defb=conv([1/250 1],[1/250 1]);
[Afb,Bfb,Cfb,Dfb]=tf2ss(nufb,defb);
[Afbb,Bfbb,Cfbb]=balreal(Afb,Bfb,Cfb);
Wfb=pck(Afbb,Bfbb,Cfbb,Dfb);
%  Define the accelerometer error weight.
nuer=(33/30)*[0.025 1];deer=[10 1];
[Aer,Ber,Cer,Der]=tf2ss(nuer,deer);
[Aerb,Berb,Cerb]=balreal(Aer,Ber,Cer);
We=pck(Aerb,Berb,Cerb,Der);
%  Perform system interconnection.
%    Inputs: wZa, wMa, wMtd, wi, wfb, etac, GyroErr, deltac.
%    Outputs:  yZa, yMa, yMtd, yi, yfb, We(etac-eta), Wc*delta, Wcd*delta_dot,
%              (etac-eta), Pitch Rate
systemnames = 'Aero Servo Wfb We';
inputvar = '[wZa; wMa; wMtd; wi; wfb; etac; e_gyro; deltac]';
outputvar = ['[1.2*Aero(2); 3.7*Aero(2); 0.7*Aero(4); 0.5*deltac;', ...
             'Servo(1); We; 1.4*Servo(1); 0.1*Servo(2);', ...
			 '30*etac-Aero(1)-Aero(3)-Wfb(1); Aero(4)+Wfb(2)+.001*e_gyro]'];
input_to_Aero='[Servo(1); wZa; wMa; wMtd]';
input_to_Servo='[deltac+wi]';
input_to_Wfb='[wfb]';
input_to_We='[30*etac-Aero(1)]';
sysoutname='P';
cleanupsysic='yes';
sysic
%  Set variables for DK-iteration algorithm.
  %  Number of measurements.
nmeas = 2;
  %  Number of controls.
ncont = 1;
  %  Block structure for the perturbation.
blk = [1 1
       1 1
       1 1
       1 1
       1 1
	   2 3];
  %  Frequency vector for evaluating the SSV.
omega = logspace(-1,4);
%  Perform DK-iteration.
DK_DEF_NAME='my_defin';
dkit
%  The DK-iteration algorithm yields a controllers for each iteration.
%    Request the desired controller.
iter=input('Enter the iteration number of the desired controller ', 's');
K=eval(['k_dk',iter,'suffix']);
