% Section 12.3 Aero-engine control, Part II
% DESIGN OF MULTIVARIABLE CONTROLLER FOR AERO ENGINE, 
% REWIEWED BY KJETIL HAVRE 13/4 - 1995.
%

clear all 
close all
% load the engine model		
load aero1
G = G5; 
				
% define frequency points	
w=logspace(-3,3, 301);

% scale the plant G_eng (Si and So are the input & output scaling matrices)
% and add actuator dynamics G_act to get the unshaped plant G
%G=mmult(So,G_eng,G_act,Si);	

% PLANT G(s). COMPUT FREQUENCY RESPONSES.
Gjw = frsp( G, w );
%vplot('liv,lm', Gjw);


% SINGULAR VALUES OF PLANT.
%Figure 12.12
figure(1);subplot(221);
vplot('liv,lm', vsvd(Gjw), 1, ':');
axis([1e-1 1e3 1e-4 1e2]);
xlabel('Frequency');drawnow


% build weight Wpbar=I/s (where I is a 3*3 identity matrix)
sys1=zp2sys([],0,1);
Wpbar=daug(sys1,sys1,sys1);
% WEIGHT CROSSOVER EQUAL TO 1 rad/second.

% build weight W2=I
W2=eye(3);

% build W2*G*Wpbar and find the align gain Ka (at 7 rad/sec)
W2GWpbar=mmult(W2,G,Wpbar);
[a,b,c,d]=unpck(W2GWpbar);
ff=c*inv(sqrt(-1)*7*eye(size(a))-a)*b+d;	
Ka=align(ff);


% find condition number of Ka. It is 3.33 (and not 3.7!)
cond(Ka)         % COMPARE WITH CONDITON NUMBER OF G(j7).
cond( ff )       % CONDITON NUMBER FOR G(j7) = 3.77

% build Kg, W1 and the final shaped plant W2*G*W1
Kg=daug(1,2.5,.3);
W1=mmult(Wpbar,Ka,Kg);
W2GW1=mmult(W2,G,W1);
Gs = W2GW1;

% plot singular values of the shaped plant in 
[a,b,c,d]=unpck(W2GW1);
Gsjw = frsp( Gs, w );
subplot(222);
vplot('liv,lm', vsvd(Gsjw), 1, ':');
axis([1e-1 1e3 1e-4 1e2]);
xlabel('Frequency');drawnow

% find gamma_0 by solving the two Riccati equations
S=eye(size(d'*d))+d'*d;
Sinv=S\eye(3);
R=eye(size(d*d'))+d*d';
Rinv=R\eye(3);
[X1,X2,XAMP,XERR,XWELLPOSED,X]=aresolv(a-b*Sinv*d'*c,c'*Rinv*c,b*Sinv*b');
if (XWELLPOSED=='FALSE')
 disp('*** WARNING! X-Riccati Equation may be ill-posed ***')
end
[Z1,Z2,ZAMP,ZERR,ZWELLPOSED,Z]=aresolv((a-b*Sinv*d'*c)',b*Sinv*b',c'*Rinv*c);
if (ZWELLPOSED=='FALSE')
 disp('*** WARNING! Z-Riccati Equation may be ill-posed ***')
end
gamma_0=sqrt(1+max(eig(X*Z)))
%
%  Initial gamma is: 2.3297
%

rootR=sqrtm(R);
H=-(b*d'+Z*c')*Rinv;

% build the reference model Mo
sys1=nd2sys(1,[.018 1]);sys2=nd2sys(1,[.008 1]);sys3=nd2sys(1,[.2 1]);
Mo=daug(sys1,sys2,sys3);
% COMPUTE AND PLOT THE REFERENCE MODEL SINGULAR VALUES.
Mojw = frsp( Mo, w );

[Ao,Bo,Co,Do]=unpck(Mo);

% set rho to 1 and build the generalized plant
rho=1;
A=daug(a,Ao);invrootR=rootR\eye(3);
B1_12=(b*d'+Z*c')*invrootR;[m1,n1]=size(B1_12);[mBo,nBo]=size(Bo);
[mb,nb]=size(b);
B1=[zeros(m1,nBo) B1_12;-Bo zeros(mBo,n1)];
B2=[b;zeros(mBo,nb)];
[mc,nc]=size(c);[mCo,nCo]=size(Co);[mDo,nDo]=size(Do);
[mR,nR]=size(rootR);[md,nd]=size(d);
C1=[zeros(nb,nc) zeros(nb,nCo);c zeros(mc,nCo);rho*c rho^2*Co];
C2=[zeros(mc,nc) zeros(mc,nCo);c zeros(mc,nCo)];
D11=[zeros(nb,nDo) zeros(nb,nR);zeros(mR,nDo) rootR;-(rho^2)*Do rho*rootR];
D12=[eye(nb,nd);d;rho*d];
D21=[rho*eye(mc,nDo) zeros(mc,nR);zeros(mR,nDo) rootR];
D22=[zeros(mc,nd);d];

% do gamma iterations and calculate the H-inf controller
% given by K=(acp,bcp,ccp,dcp). gam is the achieved suboptimal gamma.
%[gam,acp,bcp,ccp,dcp]=hinfopt_RRZZ(A,B1,B2,C1,C2,D11,D12,D21,D22,1:9,[.01 1 0.2]);
[gam,acp,bcp,ccp,dcp]=hinfopt(A,B1,B2,C1,C2,D11,D12,D21,D22,1:9,[.01 1 0.2]);
gam=1/gam
%
% Final gamma value is: 2.9091
%

% scale the prefilter of the 2-DOF controller to give perfect steady-state
% tracking

% 
%  Form the unscaled kontroller without integral action.
%  to be used in the model reduction.
%

disp('Kontroller is formed witout scaling of prefilter.')

K=pck(acp,bcp,ccp,dcp);
%save aeroK K Gs Mo


dcK1=dcgain(acp,bcp(:,1:3),ccp,dcp(:,1:3));dcK1inv=dcK1\eye(3);
dcK2=dcgain(acp,bcp(:,4:6),ccp,dcp(:,4:6));
bcp(:,1:3)=bcp(:,1:3)*dcK1inv*(-dcK2);
dcp(:,1:3)=dcp(:,1:3)*dcK1inv*(-dcK2);
K=pck(acp,bcp,ccp,dcp);

%save RobToolCont K 
%load RobToolCont

% build the final controller W1*[K1 K2]*[I 0;0 W2]
Kf=mmult(W1,K);		% W2=I so no need for post-multiplication by 
			% [I 0;0 W2]

Kf1 = sel(Kf, 1:3, 1:3);
Kf2 = sel(Kf, 1:3, 4:6);

systemnames  = 'G Kf1 Kf2';
inputvar     = '[ r(3) ]';
outputvar    = '[G]';
input_to_G   = '[Kf1+Kf2]';
input_to_Kf1 = '[r]';
input_to_Kf2 = '[G]';
sysoutname   = 'Ms';
cleanupsysic = 'yes';
sysic;

y1 = trsp(Ms, [1 0 0]', 2, 2/200);
y2 = trsp(Ms, [0 1 0]', 2, 2/200);
y3 = trsp(Ms, [0 0 1]', 2, 2/200);

%Figure 12.13
figure(2);subplot(231);
vplot(sel(y1,1,1), sel(y1,2,1), '--', sel(y1,3,1), '-.', 1, ':');
axis([0 2 -0.1 1.1])
xlabel('Time');drawnow

subplot(232);
vplot(sel(y2,1,1), sel(y2,2,1), '--', sel(y2,3,1), '-.', 1, ':');
axis([0 2 -0.1 1.1])
xlabel('Time');drawnow

subplot(233);
vplot(sel(y3,1,1), sel(y3,2,1), '--', sel(y3,3,1), '-.', 1, ':');
axis([0 2 -0.1 1.1])
xlabel('Time');drawnow

% plot the sensitivity function
L  = mmult(G,Kf2);
LI = mmult(Kf2,G);
Se = minv(msub(eye(3),L));
SI = minv(msub(eye(3),LI));
T  = msub(eye(3), Se);
TI = msub(eye(3), SI);

% Peak in S.
HinfS = hinfnorm(Se, 1e-6)
%
% Peak in S is: 1.4396 for \w = 2.7499e+01
%

%Figure 12.14
figure(3);subplot(221);
vplot('liv,lm', vsvd(frsp(Se, w)), 1, ':')
axis([1e-2 1e3 1e-3 3e0])
xlabel('Frequency')

% output multiplicative robustness

subplot(222);
vplot('liv,lm',sel(vsvd(frsp(T,w)),1,1),sel(vsvd(frsp(TI,w)),1,1),'--',1,':')
axis([1e-2 1e3 1e-3 3e0])
xlabel('Frequency')
