%Section 11.6.2 Reduction of an aero-engine controller
%
%  File to generate figures.
%
clear all
close all

% The shaped plant of the model and the unscaled controller 
% are saved in aeroK.mat from the design, in Sec12_33.m.
% Also the referencew model is inside this file.
load aeroK   
% Controller K, Shaped plant Gs and reference model Mo.

w = logspace(-4, 4, 201);
G = Gs;

Mojw = frsp( Mo, w );

K1 = sel(K, 1:3, 1:3);
K2 = sel(K, 1:3, 4:6);

rho = 1;


systemnames = 'G K1 K2';
inputvar    = '[ r(3) ]';
outputvar   = '[G]';
input_to_G  = '[K1+K2]';
input_to_K1 = '[r]';
input_to_K2 = '[G]';
sysoutname = 'M';
cleanupsysic = 'yes';
sysic;

Mjw = frsp(M, w);

%Figure 11.5
figure(1);subplot(221);
vplot('liv,lm', vsvd(Mojw), vsvd(Mjw), '--')
axis([1e-2 1e3 1e-4 5e0]); xlabel('Frequency');
drawnow

% Scale the prefilter to match Mo at steady state.
Moj0 = frsp(Mo, 0);
Mj0  = frsp(M, 0);

SM   = mmult(minv(Mj0), Moj0);
SM   = SM(1:3, 1:3);

K1s = mmult(K1, SM);

systemnames  = 'G K1s K2';
inputvar     = '[ r(3) ]';
outputvar    = '[G]';
input_to_G   = '[K1s+K2]';
input_to_K1s = '[r]';
input_to_K2  = '[G]';
sysoutname   = 'Ms';
cleanupsysic = 'yes';
sysic;

Msjw = frsp(Ms, w);

subplot(222);
vplot('liv,lm', vsvd(Mojw), vsvd(Msjw), '--')
axis([1e-2 1e3 1e-4 1e1]); xlabel('Frequency')
drawnow

% 1. Reducing the scaled controller.

% a. Balanced residualization.
Ks   = sbs(K1s, K2);
[Ksb Hs]  = sysbal(Ks);
KsBr = sresid(Ksb, 7);
sum(Hs(8:max(size(Hs))))

K1 = sel(KsBr,1:3,1:3);
K2 = sel(KsBr,1:3,4:6);

systemnames = 'G K1 K2';
inputvar    = '[ r(3) ]';
outputvar   = '[G]';
input_to_G  = '[K1+K2]';
input_to_K1 = '[r]';
input_to_K2 = '[G]';
sysoutname = 'Mbr';
cleanupsysic = 'yes';
sysic;

Mbrjw = frsp(Mbr, w);

%Figure 11.6
figure(2);subplot(231);
vplot('liv,lm', vsvd(Mojw), vsvd(Mbrjw),'--')
axis([1e-2 1e3 1e-4 1e1]); xlabel('Frequency');
drawnow

hinfnorm(msub(Mbr, Mo), 1e-7)

% b. Balanced truncation.
KsBt = strunc(Ksb, 7);

K1 = sel(KsBt,1:3,1:3);
K2 = sel(KsBt,1:3,4:6);

systemnames = 'G K1 K2';
inputvar    = '[ r(3) ]';
outputvar   = '[G]';
input_to_G  = '[K1+K2]';
input_to_K1 = '[r]';
input_to_K2 = '[G]';
sysoutname = 'Mbt';
cleanupsysic = 'yes';
sysic;

Mbtj0 = frsp(Mbt, 0);
SMt    = mmult(minv(Mbtj0), Moj0);
SMt    = SMt(1:3, 1:3);
Mbt   = mmult(Mbt, SMt);

Mbtjw = frsp(Mbt, w);

subplot(232);
vplot('liv,lm', vsvd(Mojw), vsvd(Mbtjw),'--')
axis([1e-2 1e3 1e-4 1e1]); xlabel('Frequency')
drawnow

% c. Optimal hankel norm approximation.

KsBh = hankmr(Ksb, Hs, 7, 'd');

K1 = sel(KsBh,1:3,1:3);
K2 = sel(KsBh,1:3,4:6);

systemnames = 'G K1 K2';
inputvar    = '[ r(3) ]';
outputvar   = '[G]';
input_to_G  = '[K1+K2]';
input_to_K1 = '[r]';
input_to_K2 = '[G]';
sysoutname = 'Mbh';
cleanupsysic = 'yes';
sysic;

Mbhj0 = frsp(Mbh, 0);
SMh   = mmult(minv(Mbhj0),Moj0);
SMh   = SMh(1:3, 1:3);

Mbhs = mmult(Mbh, SMh);

Mbhsjw = frsp(Mbhs, w);

subplot(233);
vplot('liv,lm', vsvd(Mojw), vsvd(Mbhsjw),'--')
axis([1e-2 1e3 1e-4 1e1]); xlabel('Frequency')
drawnow

% 2. Reduce the unscaled controller.

% a. Balance residualization.

[Kb H]  = sysbal(K);
KBr = sresid(Kb, 7);

K1 = sel(KBr,1:3,1:3);
K2 = sel(KBr,1:3,4:6);

systemnames = 'G K1 K2';
inputvar    = '[ r(3) ]';
outputvar   = '[G]';
input_to_G  = '[K1+K2]';
input_to_K1 = '[r]';
input_to_K2 = '[G]';
sysoutname = 'Mbr2';
cleanupsysic = 'yes';
sysic;

Mbr2j0 = frsp(Mbr2, 0);
SM2    = mmult(minv(Mbr2j0), Moj0);
SM2    = SM2(1:3, 1:3);
Mbr2   = mmult(Mbr2, SM2);

Mbr2jw = frsp(Mbr2, w);

%Figure 11.10
figure(3);subplot(231);
vplot('liv,lm', vsvd(Mojw), vsvd(Mbr2jw),'--')
axis([1e-2 1e3 1e-4 1e1]); xlabel('Frequency')
drawnow

% b. Balanced truncation.
KBt = strunc(Kb, 7);

K1 = sel(KBt,1:3,1:3);
K2 = sel(KBt,1:3,4:6);

systemnames = 'G K1 K2';
inputvar    = '[ r(3) ]';
outputvar   = '[G]';
input_to_G  = '[K1+K2]';
input_to_K1 = '[r]';
input_to_K2 = '[G]';
sysoutname = 'Mbt2';
cleanupsysic = 'yes';
sysic;

Mbt2j0 = frsp(Mbt2, 0);
SMt2    = mmult(minv(Mbt2j0), Moj0);
SMt2    = SMt2(1:3, 1:3);
Mbt2   = mmult(Mbt2, SMt2);

Mbt2jw = frsp(Mbt2, w);

subplot(232);
vplot('liv,lm', vsvd(Mojw), vsvd(Mbt2jw),'--')
axis([1e-2 1e3 1e-4 1e1]); xlabel('Frequency')
drawnow

% c. Optimal hankel norm approximation.

KBh = hankmr(Kb, H, 7, 'd');

K1 = sel(KBh,1:3,1:3);
K2 = sel(KBh,1:3,4:6);

systemnames = 'G K1 K2';
inputvar    = '[ r(3) ]';
outputvar   = '[G]';
input_to_G  = '[K1+K2]';
input_to_K1 = '[r]';
input_to_K2 = '[G]';
sysoutname = 'Mbh2';
cleanupsysic = 'yes';
sysic;

Mbh2j0 = frsp(Mbh2, 0);
SMh2   = mmult(minv(Mbh2j0),Moj0);
SMh2   = SMh2(1:3, 1:3);

Mbh2 = mmult(Mbh2, SMh2);

Mbh2jw = frsp(Mbh2, w);

subplot(233);
vplot('liv,lm', vsvd(Mojw), vsvd(Mbh2jw),'--')
axis([1e-2 1e3 1e-4 1e1]); xlabel('Frequency')
drawnow


% Step responses.

% 1. Full controller.

yr1 = trsp(Ms, [1 0 0]', 2, 2/200);
yr2 = trsp(Ms, [0 1 0]', 2, 2/200);
yr3 = trsp(Ms, [0 0 1]', 2, 2/200);

% 2. Balanced residualization.
ybrr1 = trsp(Mbr, [1 0 0]', 2, 2/200);
ybrr2 = trsp(Mbr, [0 1 0]', 2, 2/200);
ybrr3 = trsp(Mbr, [0 0 1]', 2, 2/200);

SimStepAxis = [0 2 -0.5 1.5];

%Figure 11.7
figure(4);subplot(231);
vplot(yr1, ybrr1,'--'); axis(SimStepAxis)
xlabel('Time');
drawnow;

subplot(232);
vplot(yr2, ybrr2, '--'); axis(SimStepAxis)
xlabel('Time');
drawnow

subplot(233);
vplot(yr3, ybrr3, '--'); axis(SimStepAxis)
xlabel('Time');
drawnow

% 3. Balanced truncation.
ybtr1 = trsp(Mbt, [1 0 0]', 2, 2/200);
ybtr2 = trsp(Mbt, [0 1 0]', 2, 2/200);
ybtr3 = trsp(Mbt, [0 0 1]', 2, 2/200);

%Figure 11.8
figure(5);subplot(231);
vplot(yr1, ybtr1, '--'); axis(SimStepAxis)
xlabel('Time');drawnow
			     
subplot(232);
vplot(yr2, ybtr2, '--'); axis(SimStepAxis)
xlabel('Time');drawnow

subplot(233);
vplot(yr3, ybtr3, '--'); axis(SimStepAxis)
xlabel('Time');
drawnow

% 4. Hankel norm approximation.
ybhsr1 = trsp(Mbhs, [1 0 0]', 2, 2/200);
ybhsr2 = trsp(Mbhs, [0 1 0]', 2, 2/200);
ybhsr3 = trsp(Mbhs, [0 0 1]', 2, 2/200);

%Figure 11.9
figure(6);subplot(231);
vplot(yr1, ybhsr1, '--'); axis(SimStepAxis)
xlabel('Time');drawnow

subplot(232);
vplot(yr2, ybhsr2, '--'); axis(SimStepAxis)
xlabel('Time');drawnow

subplot(233);
vplot(yr3, ybhsr3, '--'); axis(SimStepAxis)
xlabel('Time');drawnow


% 4. Unscaled model as starting point for model reduction.

% 2. Balanced residualization.
ybr2r1 = trsp(Mbr2, [1 0 0]', 2, 2/200);
ybr2r2 = trsp(Mbr2, [0 1 0]', 2, 2/200);
ybr2r3 = trsp(Mbr2, [0 0 1]', 2, 2/200);

%Figure 11.11
figure(7);subplot(231);
vplot(yr1, ybr2r1, '--'); axis(SimStepAxis)
xlabel('Time');drawnow;

subplot(232);
vplot(yr2, ybr2r2, '--'); axis(SimStepAxis)
xlabel('Time');
drawnow

subplot(233);
vplot(yr3, ybr2r3, '--'); axis(SimStepAxis)
xlabel('Time');drawnow

% 2. Balanced truncation.
ybt2r1 = trsp(Mbt2, [1 0 0]', 2, 2/200);
ybt2r2 = trsp(Mbt2, [0 1 0]', 2, 2/200);
ybt2r3 = trsp(Mbt2, [0 0 1]', 2, 2/200);

%Figure 11.12
figure(8);subplot(231);
vplot(yr1, ybt2r1, '--'); axis(SimStepAxis)
xlabel('Time');drawnow

subplot(232);
vplot(yr2, ybt2r2, '--'); axis(SimStepAxis)
xlabel('Time');
drawnow

subplot(233);
vplot(yr3, ybt2r3, '--'); axis(SimStepAxis)
xlabel('Time');
drawnow

% 4. Hankel norm approximation.
ybh2r1 = trsp(Mbh2, [1 0 0]', 2, 2/200);
ybh2r2 = trsp(Mbh2, [0 1 0]', 2, 2/200);
ybh2r3 = trsp(Mbh2, [0 0 1]', 2, 2/200);

%Figure 11.13
figure(9);subplot(231);
vplot(yr1, ybh2r1, '--'); axis(SimStepAxis)
xlabel('Time');drawnow

subplot(232);
vplot(yr2, ybh2r2, '--'); axis(SimStepAxis)
xlabel('Time');drawnow

subplot(233);
vplot(yr3, ybh2r3, '--'); axis(SimStepAxis)
xlabel('Time');
drawnow
