%Section 12.2 Helicopter control
%April 2014: Note that this files uses obseolete Matlab commands. see 2nd edition (Sec13_2.m) for updated matlab files,
%
clear
clc
disp('*********************************************************************')
disp('*         H-infinity Design for Gust Turbulence  Rejection          *')
disp('*                                                                   *')
disp('*         Also, H-infinity Mixed Sensitivity Design                 *')
disp('*                                                                   *')
disp('*********************************************************************')
disp('*                                                                   *')
disp('*             Further Description in the Paper:                     *')
disp('*             Rotorcraft Control Law Design for Rejection of        *')
disp('*             Atmospheric Turbulence                                *')
disp('*                  I.Postlethwaite, N.P.Foster, D.J.Walker          *')
disp('*                  1994 Control94, Warwick, May 1994.               *')
disp('*                                                                   *')
disp('*             Written by Neale P. Foster                            *')
disp('*             Control Systems Research                              *')
disp('*             Engineering Department                                *')
disp('*             Leicester University                                  *')
disp('*             University Road                                       *')
disp('*             Leicester LE1 7RH                                     *')
disp('*             England                                               *')
disp('*     email: (Leicester University): npf@sun.engg.le.ac.uk          *')
disp('*                        or                                         *')
disp('*     email: (British Aerospace, Bristol)neale.foster@def.bae.co.uk *')
disp('*********************************************************************')

%---------------------------------------------------------------------------
disp('******* helicopter matrices -- Unscaled plt')

 
a01 = [          0                  0                  0   0.99857378005981;
                 0                  0   1.00000000000000  -0.00318221934140;
                 0                  0 -11.57049560546880  -2.54463768005371;
                 0                  0   0.43935656547546  -1.99818229675293;
                 0                  0  -2.04089546203613  -0.45899915695190;
-32.10360717773440                  0  -0.50335502624512   2.29785919189453;
  0.10216116905212  32.05783081054690  -2.34721755981445  -0.50361156463623;
 -1.91097259521484   1.71382904052734  -0.00400543212891  -0.05741119384766];

a02 = [0.05338427424431             0                  0                  0;
  0.05952465534210                  0                  0                  0;
 -0.06360262632370   0.10678052902222  -0.09491866827011   0.00710757449269;
                 0   0.01665188372135   0.01846204698086  -0.00118747074157;
 -0.73502779006958   0.01925575733185  -0.00459562242031   0.00212036073208;
                 0  -0.02121581137180  -0.02116791903973   0.01581159234047;
  0.83494758605957   0.02122657001019  -0.03787973523140   0.00035400385968;
                 0   0.01398963481188  -0.00090675335377  -0.29051351547241];

a0=[a01 a02];

b0=[              0                  0                  0                  0;
                  0                  0                  0                  0;
   0.12433505058289   0.08278584480286  -2.75247764587402  -0.01788876950741;
  -0.03635892271996   0.47509527206421   0.01429074257612                  0;
   0.30449151992798   0.01495801657438  -0.49651837348938  -0.20674192905426;
   0.28773546218872  -0.54450607299805  -0.01637935638428                  0;
  -0.01907348632812   0.01636743545532  -0.54453611373901   0.23484230041504;
  -4.82063293457031  -0.00038146972656                  0                 0];

c0 = [ 0        0         0         0         0    0.0595   0.05329  -0.9968;
     1.0        0         0         0         0         0         0        0;
       0      1.0         0         0         0         0         0        0;
       0        0         0  -0.05348       1.0         0         0        0;
       0        0       1.0         0         0         0         0        0;
       0        0         0       1.0         0         0         0       0];

d0 = zeros(6,4);

%----------------------------------------------------------------------------
disp('*******Constructing Weights')

%  aw1

n11=0.5*[1 12];
d11=[1 0.012];

n12=0.89*[1 2.81];
d12=[1 0.005];

n13=0.89*[1 2.81];
d13=[1 0.005];

n14=0.5*[1 10];
d14=[1 0.01];

n15=2*[1 0];
d15=[1 8.5 18];

n16=2*[1 0];
d16=[1 8.5 18];

[aw11,bw11,cw11,dw11]=tf2ss(n11,d11);
[aw12,bw12,cw12,dw12]=tf2ss(n12,d12);
[aw13,bw13,cw13,dw13]=tf2ss(n13,d13);
[aw14,bw14,cw14,dw14]=tf2ss(n14,d14);
[aw15,bw15,cw15,dw15]=tf2ss(n15,d15);
[aw16,bw16,cw16,dw16]=tf2ss(n16,d16);

aw1=daug(aw11,aw12,aw13,aw14,aw15,aw16);
bw1=daug(bw11,bw12,bw13,bw14,bw15,bw16);
cw1=daug(cw11,cw12,cw13,cw14,cw15,cw16);
dw1=daug(dw11,dw12,dw13,dw14,dw15,dw16);

% aw2

n21=0.5*[1 0.0001];
d21=[1 10];

n22=0.5*[1 0.0001];
d22=[1 10];

n23=0.5*[1 0.0001];
d23=[1 10];

n24=0.5*[1 0.0001];
d24=[1 10];

[aw21,bw21,cw21,dw21]=tf2ss(n21,d21);
[aw22,bw22,cw22,dw22]=tf2ss(n22,d22);
[aw23,bw23,cw23,dw23]=tf2ss(n23,d23);
[aw24,bw24,cw24,dw24]=tf2ss(n24,d24);

% Error corrected Sept.01 based on input from Robert.Josselson@itt.com:
aw2=daug(aw21,aw22,aw23,aw24);
bw2=daug(bw21,bw22,bw23,bw24);
cw2=daug(cw21,cw22,cw23,cw24);
dw2=daug(dw21,dw22,dw23,dw24);

%  dw3
aw3=[];bw3=[];cw3=[];
cw3=zeros(6,0); % Changed April 2014 to make file run, BUT you should use 2nd edition files instead (Sec13_2.m)
dw3=daug(1,1,1,1,0.1,0.1);

% Sizes
[n,n]=size(a0);
[p,m]=size(d0);
[n1,n1]=size(aw1);
[n2,n2]=size(aw2);
[n3,n3]=size(aw3);
%----------------------------------------------------------------------------
% Output Scaling
scaleyue=daug(0.1,5,2,2,2,5);
c0sc=scaleyue*c0;

ki=menu('Select the Design Method','H-infinity Mixed Sensitivity Design',...
        'Disturbance Rejection Design');

if ki==1 

%----------------------------------------------------------------------------
disp('**Standard H-infinity Mixed Sensitivity Design - No Extra Disturbance')


disp('Forming Standard Plant Configuration')

A=[ a0 zeros(n,n1+n2+n3);
    -bw1*c0sc aw1 zeros(n1,n2) bw1*cw3;
    zeros(n2,n+n1) aw2 zeros(n2,n3);
    zeros(n3,n+n1+n2) aw3];

B1=[zeros(n,p);
    bw1*dw3;
    zeros(n2,p);
    bw3];

B2=[ b0;
   -bw1*d0;
     bw2;
     zeros(n3,m)];

C1=[-dw1*c0sc cw1 zeros(p,n2) dw1*cw3;
    zeros(m,n+n1) cw2 zeros(m,n3); 
    c0sc zeros(p,n1+n2+n3)];

C2=[-c0sc zeros(p,n1+n2) cw3];

D11=[dw1*dw3;
     zeros(m,p); 
     zeros(p,p)];

D12=[dw1*d0;
     dw2;
      d0];

D21=dw3;

D22=-d0;

else

%----------------------------------------------------------------------------
disp('*******Disturbance Representation')

Bg=a0(:,(6:8));
disp('******* multiplying Bg by 30')
Bg=Bg.*30;
[bg1,bg2]=size(Bg);

disp('Forming Standard Plant Configuration')

A=[ a0 zeros(n,n1+n2+n3);
    -bw1*c0sc aw1 zeros(n1,n2) bw1*cw3;
    zeros(n2,n+n1) aw2 zeros(n2,n3);
    zeros(n3,n+n1+n2) aw3];

B1=[zeros(n,p) Bg;
    bw1*dw3 zeros(bg1,bg2);
    zeros(n2,p) zeros(n2,bg2);
    bw3 zeros(n3,bg2)];

B2=[ b0;
    -bw1*d0;
     bw2;
     zeros(n3,m)];

C1=[-dw1*c0sc cw1 zeros(p,n2) dw1*cw3;
    zeros(m,n+n1) cw2 zeros(m,n3); 
    c0sc zeros(p,n1+n2+n3)];

C2=[-c0sc zeros(p,n1+n2) cw3];

D11=[dw1*dw3 zeros(p,bg2);
     zeros(m,p) zeros(m,bg2); 
     zeros(p,p) zeros(p,bg2)];

D12=[dw1*d0;
     dw2; 
      d0];

D21=[dw3 zeros(p,bg2)];

D22=-d0;

end
%----------------------------------------------------------------------------
disp('*******H-infinity solving with gamma of 4')
tol=0.2;
gmin=4;
gmax=4;   

p=pck(A,[B1 B2],[C1;C2],[D11 D12;D21 D22]);
[k,g,gfin,ax,ay,hamx,hamy]=hinfsyn(p,6,4,gmin,gmax,tol,2);
[ak,bk,ck,dk]=unpck(k);

%----------------------------------------------------------------------------
% Combine Original Output Scaling With Controller
bk=bk*scaleyue;

%----------------------------------------------------------------------------
% Cascade Controller With Original Plant
[as,bs,cs,ds]=series(ak,bk,ck,dk,a0,b0,c0,d0);

%----------------------------------------------------------------------------
% Closed Loop System
[ac,bc,cc,dc]=feedbk(as,bs,cs,ds,2);

%----------------------------------------------------------------------------
% Sensitivity: S = inv(I+GK), and KS
[asen,bsen,csen,dsen]=feedbk(as,bs,cs,ds,1);
[acs,bcs,ccs,dcs]=series(asen,bsen,csen,dsen,ak,bk,ck,dk);

%----------------------------------------------------------------------------
% Singular Values for Frequency Response Evaluation

w=logspace(-3,3,100);
svrob=sigma(ac,bc,cc,dc,w);
svrob=svrob((1:4),:); 
svsen=sigma(asen,bsen,csen,dsen,w);
svgsr=sigma(acs,bcs,ccs,dcs,w);

subplot(111);clg;
%Figure 12.3 (a) or 12.5 (a)
figure(1)
plot(log10(w),db(svsen));
        grid
	title('inv(I+GK)')
	ylabel('mag (dB)')
	xlabel('rad/s')


figure(2)
plot(log10(w),db(svrob));
        grid
	title('GK.inv(I+GK)')
	ylabel('mag (dB)')
	xlabel('rad/s')

%Figure 12.3 (b) or 12.5 (b)
figure(3)
plot(log10(w),db(svgsr));
        grid
	title('K.inv(I+GK)')
	ylabel('mag (dB)')
	xlabel('rad/s')

