%Example 10.7
% Distillation column model with 5 states and disturbances

A = [ -5.131e-3 0 0 0 0; 0 -7.366e-2 0 0 0; 0 0 -1.829e-1 0 0;
       0 0 0 -4.620e-1  9.895e-1; 0 0 0 -9.895e-1 -4.620e-1]

B = [-.629 .624; .055 -0.172; 0.030 -0.108; 
      -0.186 -0.139; -1.23 -0.056]


C= [-0.7223 -0.5170 0.3386 -0.1633e-1 0.1121;
    -0.8913 0.4728 0.9876 0.8425 0.2186]

D = [ 0 0; 0 0]


Bd = [-0.062 -0.067; 0.131 0.040; 0.022 -0.106; 
     -0.188 0.027; -0.045 0.014]

G = pck(A,B,C,D);
Gd = pck(A,Bd,C,D);

% Change the disturbance magnitudes from 30 to 20% for d1 and
% from 40 to 20% for d2
Dmag = [0.2/0.3 0; 0 0.2/0.4];
Gd = mmult(Gd,Dmag);
[Ad,Bd,Cd,Dd]=unpck(Gd);
w = logspace(-3,1,41);
Gf = frsp(G,w); Gdf=frsp(Gd,w);

% Steady-state analysis
G0 = -C*inv(A)*B + D
Gd0 = -Cd*inv(Ad)*Bd + Dd
% Actually we will use these slightly different gains, see also book p. 495;
G0 = [87.8  -86.4 ; 108.2  -109.6];
Gd0 = [7.88  8.81 ; 11.72 11.19];

% Evaluate partial control

% 1. y2=xb controlled using u2=V
g12=sel(G0,1,2);   % uncontrolled output, controlled input 
g22=sel(G0,2,2);   % controlled output, controlled input
gd1=sel(Gd0,1,':');% uncontrolled output
gd2=sel(Gd0,2,':');% controlled output
pdg1 = msub(gd1,mmult(g12,minv(g22),gd2));

% 2. y2=xb controlled using u1=L
g12=sel(G0,1,1);   % uncontrolled output, controlled input 
g22=sel(G0,2,1);   % controlled output, controlled input
gd1=sel(Gd0,1,':');% uncontrolled output
gd2=sel(Gd0,2,':');% controlled output
pdg2 = msub(gd1,mmult(g12,minv(g22),gd2));

% 3. y1=yd is controlled using u2=V
g12=sel(G0,2,2);   % uncontrolled output, controlled input 
g22=sel(G0,1,2);   % controlled output, controlled input
gd1=sel(Gd0,2,':');% uncontrolled output
gd2=sel(Gd0,1,':');% controlled output
pdg3 = msub(gd1,mmult(g12,minv(g22),gd2));

%  4. y1=yd is controlled using u1=L
g12=sel(G0,2,1);   % uncontrolled output, controlled input 
g22=sel(G0,1,1);   % controlled output, controlled input
gd1=sel(Gd0,2,':');% uncontrolled output
gd2=sel(Gd0,1,':');% controlled output
pdg4 = msub(gd1,mmult(g12,minv(g22),gd2));

pdg = abv(pdg1,pdg2,pdg3,pdg4)


% Evaluate partial control using dynamic model
figure(1)
% y2=xb controlled using u2=V
g12=sel(Gf,1,2);   % uncontrolled output, controlled input
g22=sel(Gf,2,2);   % controlled output, controlled input
gd1=sel(Gdf,1,':');% uncontrolled output
gd2=sel(Gdf,2,':');% controlled output
pdg = msub(gd1,mmult(g12,minv(g22),gd2));
subplot(2,2,1): vplot('liv,lm',pdg,1,':');
title('y2=xb controlled using u2=V');
axis([0.001,10,0.01,10]);
% y2=xb controlled using u1=L
g12=sel(Gf,1,1);   % uncontrolled output, controlled input
g22=sel(Gf,2,1);   % controlled output, controlled input
gd1=sel(Gdf,1,':');% uncontrolled output
gd2=sel(Gdf,2,':');% controlled output
pdg = msub(gd1,mmult(g12,minv(g22),gd2));
subplot(2,2,2): vplot('liv,lm',pdg,1,':');
title('y2=xb controlled using u1=L');
axis([0.001,10,0.01,10]);
%
g12=sel(Gf,2,2);   % uncontrolled output, controlled input
g22=sel(Gf,1,2);   % controlled output, controlled input
gd1=sel(Gdf,2,':');% uncontrolled output
gd2=sel(Gdf,1,':');% controlled output
pdg = msub(gd1,mmult(g12,minv(g22),gd2));
subplot(2,2,3): vplot('liv,lm',pdg,1,':');
title('y1=yd controlled using u2=V');
axis([0.001,10,0.01,10]);
%
g12=sel(Gf,2,1);   % uncontrolled output, controlled input
g22=sel(Gf,1,1);   % controlled output, controlled input
gd1=sel(Gdf,2,':');% uncontrolled output
gd2=sel(Gdf,1,':');% controlled output
pdg = msub(gd1,mmult(g12,minv(g22),gd2));
subplot(2,2,4): vplot('liv,lm',pdg,1,':');
title('y1=yd controlled using u1=L');
axis([0.001,10,0.01,10]);drawnow;
% Conclusion: Use V to control xb, yd uncontrolled,
% feed tank: no help since only affects high frequency
% so install feedforward control actiom


g11=sel(Gf,1,1);  
g12=sel(Gf,1,2); 
g21=sel(Gf,2,1);
g22=sel(Gf,2,2);
gd1=sel(Gdf,1,1);
gd2=sel(Gdf,2,1);% controlled output
pdg = msub(gd1,mmult(g12,minv(g22),gd2));
gpu = msub(g11,mmult(g12,minv(g22),g21));

% 1. Feedforward only for output 1 based on G and Gd (make things worse)
%  kd1 = mmult(minv(sel(G0,1,1)),sel(Gd0,1,1))
%  Kd = daug(kd1,0);
% 2. Steady-state feedforward for output 1 for disturbance 1: 
Kd0 = mmult(minv(G0),Gd0); 
kd0 = sel(Kd0,1,1); % get Kd0 equal to 0.541.
Pd0 = msub(pdg,mmult(gpu,kd0));
%  With 20% error
kd0e =1.2*kd0; Pd0err = msub(pdg,mmult(gpu,kd0e));
% 3. Implement only for F and output 1 (works OK, but not likely in practice)
lag = nd2sys(1,[3 1]); kd=mmult(1.2,kd0,lag); % 20 % erroR
kdf=frsp(kd,w);
Pdferr = msub(pdg,mmult(gpu,kdf));
%Figure 10.7
figure(2)
subplot(1,1,1); vplot('liv,lm',gd1,pdg,Pd0,Pd0err,Pdferr,1,':');
axis([0.001,10,0.01,10]);
%title('Effect of disturbance 1 on output 1');
text(.01,5,'1. Open loop Gd');
text(0.0013,1.7,'2. Close loop 2 Pd');
text(0.0013,0.38,'4. 20p error in FF');
text(0.0013,0.20,'5. Add filter to FF');
text(0.005,0.02,'3. Add static Feed-Forward');
xlabel('Frequency');ylabel('Magnitude');


