%****************************************************************
%      CONTROLLABILITY STUDY OF A DISTILLATION COLUMN
%                - USING CONTROL TOOLBOX
%
% H.K. Engelien, NTNU, 06/06/2001
%****************************************************************

% Load the linearised, scaled model:
%--------------------------------------
load G4;
B = B(:,[1,2,5,6,3,4]);         % Changing the column order of B matrix
D = D(:,[1,2,5,6,3,4]);         % Changing the column order of D matrix

% Create the continuous-time state-space model:
%----------------------------------------------
SYSC = ss(A,B,C,D);
size(SYSC)

% Compute the poles and pole directions:
%---------------------------------------
p = pole(SYSC);

[R DD1] = eig(A);               % Calculate right eigenvectors (R)
[L DD2] = eig(A');              % Calculate left eigenvectors (L)
yp = C*R;                       % Output pole direction
up = B'*L;                      % Input pole direction

upn = up'./(sqrt(sum(up.*up)')*ones(1,6)); % Normalised input directions
ypn = yp'./(sqrt(sum(yp.*yp)')*ones(1,4)); % Normalised output directions

[[1:1:size(p,1)]' p ypn]      % Returns poles and output directions
[eig(A') upn]                 % Returns eigenvalues and input directions

% Calculate the zeros:
%---------------------
z = zero(SYSC);

% Compute and plot the poles and zeros:
%---------------------------------------
figure(1); 
pzmap(SYSC);
hold;

% Close the level control loops:
%-------------------------------
% - have used D and B to manipulate the holdup in condenser and reboiler

K = diag([-10, -10]);               % Two proportional controllers
sysc = lft(SYSC,-K,2,2);
size(sysc)

% Split the "new" plant into manipulable variables and disturbances
% --> get : y = G u + G_d d

a = sysc.a;
b = sysc.b;
c = sysc.c;
d = sysc.d;

G  = ss(a,b(:,1:2),c,d(:,1:2));
Gd = ss(a,b(:,3:4),c,d(:,3:4));

% Plot the singular values as a function of frequency:
%------------------------------------------------------
w = logspace(-3,1);            % Creates a logarithmically spaced vector
[sv,w] = sigma(G,w);

figure(2); loglog(w,sv,w,ones(50,1),'k--');
title('Singular Values'); legend('\sigma_1', '\sigma_2')
ylabel('Magnitude'); xlabel('Frequency')
hold;

% Calculate the frequency response:
%-----------------------------------
gw  = freqresp(G,w);              % Process frequency response
gwd = freqresp(Gd,w);             % Disturbance frequency response

% Calculate the steady state gain and steady state RGA:
%--------------------------------------------------------
gw0  = evalfr(G,0)                % Relative gain at steady state
gwd0 = evalfr(Gd,0)               % Disturbacne gain at steady state

RGA0 = gw0.*(inv(gw0))'           % Steady state RGA

% Make Bode plot - process:
%---------------------------
[Gmag, Gphase] = bode(G,w);
Gmag_w   = [];
Gphase_w = [];
for i = 1:50  
    Gmag_w   = [Gmag_w; Gmag(1,1,i) Gmag(1,2,i)...
                Gmag(2,1,i) Gmag(2,2,i)];
    Gphase_w = [Gphase_w; Gphase(1,1,i) Gphase(1,2,i)...
                Gphase(2,1,i) Gphase(2,2,i)];
end

figure(3); loglog(w,Gmag_w,w,ones(50,1),'k--');
title('Process gains as a function of frequency');
ylabel('Magnitude'); xlabel('Frequency');
legend('g_{11}','g_{12}','g_{21}','g_{22}',3);
hold

figure(4); 
semilogx(w,Gphase_w(:,[1 3]),w,Gphase_w(:,[2 4])-180,w,-180*ones(50,1),'k--')
title('Process phase as a function of frequency')
ylabel('Phase'); xlabel('Frequency');
legend('g_{11}','g_{21}','g_{12}','g_{22}',3);
axis([0.001 10 -200 0]);
hold;

% Make Bode plot - disturbances:
%---------------------------------
[Gdmag, Gdphase] = bode(Gd,w);
Gd_mag_w   = [];
Gd_phase_w = [];
for i = 1:50  
    Gd_mag_w   = [Gd_mag_w; Gdmag(1,1,i) Gdmag(1,2,i)...
                  Gdmag(2,1,i) Gdmag(2,2,i)];
    Gd_phase_w = [Gd_phase_w; Gdphase(1,1,i) Gdphase(1,2,i)...
                  Gdphase(2,1,i) Gdphase(2,2,i)];
end

figure(5); loglog(w,Gd_mag_w,w,ones(50,1),'k--');
title('Disturbance gains as a function of frequency');
ylabel('Magnitude'); xlabel('Frequency');
legend('g_{d,11}','g_{d,12}','g_{d,21}','g_{d,22}',3);
hold;

figure(6); semilogx(w,Gd_phase_w,w,-180*ones(50,1),'k--');
title('Disturbance phase as a function of frequency')
ylabel('Phase'); xlabel('Frequency');
legend('g_{d,11}','g_{d,12}','g_{d,21}','g_{d,22}');
axis([0.001 10 -200 0]);
hold;

% RGA as a function of frequency : RGA = G(wj) x inv(G(wj))'
%------------------------------------------------------------

Y = [];
for i = 1:50
    RGAw = gw(:,:,i).*(inv(gw(:,:,i)))';   
    Y    = [Y; RGAw]; 
end
RGA = [Y((1:2:100),1) Y((2:2:100),1) Y((1:2:100),2) Y((2:2:100),2)];

figure(7); loglog(w,abs(RGA(:,1:2)),w,ones(50,1),'k--');
title('RGA as a Function of Frequency');
ylabel('Magnitude'); xlabel('Frequency');
legend('\lambda_{11}','\lambda_{21}');
hold;

% Input saturation : inv(G(wj))*Gd(wj)
%--------------------------------------

Y = [];
for i = 1:50
    y = inv(gw(:,:,i))*gwd(:,:,i);
    Y = [Y;y];
end

Ginv_Gd = [abs(Y(1:2:100,1)) abs(Y(2:2:100,1))...
           abs(Y(1:2:100,2)) abs(Y(2:2:100,2))];

figure(8); loglog(w,Ginv_Gd,w,ones(50,1),'k--');
title('G^{-1}G_d(\beta)');
ylabel('Magnitude'); xlabel('Frequency');
legend('\beta_{11}','\beta_{21}','\beta_{12}','\beta_{22}')
axis([0.001 10 0.01 10]);
hold;

% Performance realtive gain array (PRGA) as a function of frequency:
%--------------------------------------------------------------------
Y = [];
for i = 1:50
    y = diag([diag(gw(:,:,i))])*inv(gw(:,:,i));
    Y = [Y;y];
end

PRGA = [abs(Y(1:2:100,1)) abs(Y(2:2:100,1))...
        abs(Y(1:2:100,2)) abs(Y(2:2:100,2))];

figure(9); loglog(w,PRGA,w,ones(50,1),'k--');
title('PRGA-elements');
xlabel('Frequency'); ylabel('Magnitude');
legend('\gamma_{11}','\gamma_{21}','\gamma_{12}','\gamma_{22}',4);
hold;

% Closed loop disturbance gain (CLDG) as a function of frequency:
%-----------------------------------------------------------------
Y = [];
for i = 1:50
    y = diag([diag(gw(:,:,i))])*inv(gw(:,:,i))*gwd(:,:,i);
    Y = [Y;y];
end
CLDG = [abs(Y(1:2:100,1)) abs(Y(2:2:100,1))...
        abs(Y(1:2:100,2)) abs(Y(2:2:100,2))];

figure(10); loglog(w,CLDG,w,ones(50,1),'k--');
title('CLDG-elements');
xlabel('Frequency'); ylabel('Magnitude');
legend('g_{d11}','g_{d21}','g_{d12}','g_{d22}',1)
axis([0.001 10 0.1 100]);
hold;

% Closing the final control loops and observing control performance:
%--------------------------------------------------------------------
k1 = tf(0.261*[3.76 1],[3.76 1.e-4]);
k2 = tf(-.375*[3.31 1],[3.31 1.e-4]);
K  = ss([k1 0;0 k2]);

GK = G*K;
S  = inv(eye(2)+GK);

SGd= S*Gd;

T  = [0:1:100];
U  = [ones(50,1) zeros(50,1); ones(51,1) ones(51,1)];

figure(11);
lsim(SGd,U,T)


