%% ME-422 Multivariable control: exercise session on CSTR - solutions
% In this exercise, we consider the continuous-time nonlinear dynamics of the CSTR described by the following state-space equations:
% \dot{c} \left(t\right)=\frac{q}{V}\left(c_f \left(t\right)-c\left(t\right)\right)-k_0 \;e^{-\frac{E}{R\;T\left(t\right)}} c\left(t\right),
% \dot{T} \left(t\right)=\frac{U\;A}{V\;\rho {\;c}_p \;}\left(T_c \left(t\right)-T\left(t\right)\right)+\frac{q}{V}\left(T_f -T\left(t\right)\right)-\frac{\Delta H}{\rho \;c_p }k_0 \;e^{-\frac{E}{R\;T\left(t\right)}} c\left(t\right).
% We start by cleaning the workspace and loading the data of the CSTR

clc; close all; clear; 
% Load data
run data_CSTR.m

%
%% Point 1-8: Load data from previous exercise sessions

run('Ex3B.mlx');
run('Ex10B.mlx');
disp("-------------------------------")
disp("-------------------------------")
disp("Exercise 11B:")

%
%% Point 9a: system model with disturbances
% We linearize and discretize the new system model with
% states: \delta c\left(t\right), \delta T\left(t\right)
% controlable inputs: {\delta c}_f \left(t\right), {\delta T}_c \left(t\right)
% disturbance inputs: \delta q\left(t\right), {\delta T}_f \left(t\right)
% outputs: \delta c\left(t\right), \delta T\left(t\right)
% For this, we create a new Simulink model as well as the Interpreted matlab function that accounts for these new inputs (see files CSTR_ol_dist.slx and cstr_dist_fun.m).
% We add the time-based linearization block and we linearize the system around the same operation point as before i.e. around \left(\bar{c} ,\bar{T} \right) with inputs \left({\bar{c} }_f ,{\bar{T} }_c ,\bar{q} ,{\bar{T} }_f \right).

% Define the operation point for the new inputs
qbar = q; qbar_sim = qbar; 
Tfbar = Tf; Tfbar_sim = Tfbar;
% Simulate the system
sim('CSTR_ol_dist')
A3 = CSTR_ol_dist_Timed_Based_Linearization.a;
B3 = CSTR_ol_dist_Timed_Based_Linearization.b;
C3 = CSTR_ol_dist_Timed_Based_Linearization.c;
D3 = CSTR_ol_dist_Timed_Based_Linearization.d;

% We define the continuous-time state-space model

sys3 = ss(A3,B3,C3,D3);

% and convert it into a discrete-time model

sysd3 = c2d(sys3, Ts);

% Then, the matrices characterizing the linear discretized system are

Ad3 = sysd3.A; Bd3 = sysd3.B(:,1:2); Cd3 = sysd3.C; Dd3 = sysd3.D(:,1:2);
Md3 = sysd3.B(:,3:4); Nd3 = sysd3.D(:,3:4);

% and the dynamics are (D_{\textrm{d3}} =0 and N_{\textrm{d3}} =0)
% \left\lbrack \begin{array}{c}
% \delta c^+ \\
% \delta T^+
% \end{array}\right\rbrack =A_{\textrm{d3}} \;\left\lbrack \begin{array}{c}
% \delta c\\
% \delta T
% \end{array}\right\rbrack +B_{\textrm{d3}} \left\lbrack \begin{array}{c}
% {\delta c}_f \\
% \delta T_c
% \end{array}\right\rbrack +M_{\textrm{d3}} \left\lbrack \begin{array}{c}
% \delta q\\
% \delta T_f
% \end{array}\right\rbrack,
% y=C_{\textrm{d3}} \left\lbrack \begin{array}{c}
% \delta c\\
% \delta T
% \end{array}\right\rbrack +N_{\textrm{d3}} \left\lbrack \begin{array}{c}
% \delta q\\
% \delta T_f
% \end{array}\right\rbrack.
%
%% Point 9b: controller with integral action
% As seen in the lectures, for designning an LTI controller for sysd3 using pole placement, a necessary condition is that \textrm{det}\left(\Sigma \;\right)=\textrm{det}\left(\left\lbrack \begin{array}{cc}
% A_{\textrm{d3}} -I & B_{\textrm{d3}} \\
% C_{\textrm{d3}}  & 0
% \end{array}\right\rbrack \right)\not= 0

det_Sigma = det([Ad3-eye(2) Bd3 ; Cd3 zeros(2,2)])
fprintf("Since Sigma is non-singular, reachability of this augmented system\n" + ...
    "follows from reachability of the original system.\n\n")

% As seen in point 7a, the original system is reachable. Thus, so is the augmented system.
% For designing the controller with integral action, we define {\bar{A} }_3 =\left\lbrack \begin{array}{cc}
% A_{\textrm{d3}}  & 0\\
% -C_{\textrm{d3}}  & I
% \end{array}\right\rbrack, {\bar{B} }_3 =\left\lbrack \begin{array}{c}
% B_{\textrm{d3}} \\
% 0
% \end{array}\right\rbrack, {\bar{M} }_3 =\left\lbrack \begin{array}{c}
% M_{\textrm{d3}} \\
% N_{\textrm{d3}}
% \end{array}\right\rbrackand {\bar{C} }_3 =\left\lbrack \begin{array}{cc}
% 0 & I
% \end{array}\right\rbrack.

Abar3 = [Ad3 zeros(2,2) ; -Cd3 eye(2)];
Bbar3 = [Bd3 ; zeros(2,2)];
Mbar3 = [Md3 ; Nd3];
Cbar3 = [zeros(2,2) eye(2)];

%
% Then, we can design the desired controller with poles at [0.85, 0.85, 0.5, 0.5] as per

Kr = place(Abar3,Bbar3, [0.85, 0.85, 0.5, 0.5]);
Kx = Kr(:, 1:2);
Ku = Kr(:, 3:4);

%
% For simulating the system (both the linearized and the nonlinear models), we implement them in a new Simulink file dubbed CSTR_int_ac.slx.
% We set the initial conditions for the system and integrators

x0 = [0; 0]; % \delta[c,T]
xi0 = [0; 0]; % integrators

% and define the 5% perturbations on the disturbances

q_pert = 0.05 * q;
Tf_pert = 0.05 * Tf;

% Note that, as set in the Simulink model, the perturbations act at t=25\;\mathrm{s} and t=40\;\mathrm{s}.
% The states to be tracked consist of pulse signals over the operation state values, with amplitude 2% of the operation values

deltac_ref = 0.015 * cbar;
deltaT_ref = 0.015 * Tbar;

%
% We are ready to run the simulations

out_ia = sim("CSTR_int_ac.slx");

% and plot the results

figure
sgtitle("Pole placement - perturbation at t = 25s and t = 40s")
subplot(211); hold on
plot(out_ia.c)
plot(out_ia.c_ref,'-.', 'LineWidth', 1)
xlabel('t'); ylabel('\deltac(t)')
legend("linearized", "nonlinear", "reference", 'Location', 'southwest')
subplot(212); hold on
plot(out_ia.T)
plot(out_ia.T_ref,'-.', 'LineWidth', 1)
xlabel('t'); ylabel('\deltaT(t)')
legend("linearized", "nonlinear", "reference", 'Location', 'southwest')
fprintf("The reference changes at t = [0, 15, 30, 45]s.\n" + ...
    "The perturbation is added at time t = 25s and t = 40s.\n\n")

%
%% Point 10: LQR
% We define the Q and R matrices for the LQR controller

Q = diag([1/cbar, 1/Tbar, 1/cbar, 1/Tbar]);
R = diag([1/cfbar, 1/Tcbar]);

% For the design of the LQR controller, we use the same system as before, i.e. the discretized and linearized around \left(\bar{c} ,\bar{T} \right) with inputs \left({\bar{c} }_f ,{\bar{T} }_c ,\bar{q} ,{\bar{T} }_f \right)with:
% states: \delta c\left(t\right), \delta T\left(t\right)
% controlable inputs: {\delta c}_f \left(t\right), {\delta T}_c \left(t\right)
% disturbance inputs: \delta q\left(t\right), {\delta T}_f \left(t\right)
% outputs: \delta c\left(t\right), \delta T\left(t\right)
% Using the function dlqr, we obtain the LQR controller

KLQ = dlqr(Abar3, Bbar3, Q, R);
Kx = KLQ(:, 1:2);
Ku = KLQ(:, 3:4);

%
% We use the same Simulink model (CSTR_int_ac.slx) for running the simulations over the linearized and nonlinear systems. Moreover, we also use the same input signals as defined previously.

outLQi = sim("CSTR_int_ac.slx");

% We plot the obtained trajectories:

figure
sgtitle("LQR - perturbation at t = 25s and t = 40s")
subplot(211); hold on
plot(outLQi.c)
plot(outLQi.c_ref,'-.', 'LineWidth', 1)
xlabel('t'); ylabel('\deltac(t)')
legend("linearized", "nonlinear", "reference", 'Location', 'southwest')
subplot(212); hold on
plot(outLQi.T)
plot(outLQi.T_ref,'-.', 'LineWidth', 1)
xlabel('t'); ylabel('\deltaT(t)')
legend("linearized", "nonlinear", "reference", 'Location', 'southwest')
fprintf("The reference changes at t = [0, 15, 30, 45]s.\n" + ...
    "The perturbation is added at time t = 25s and t = 40s.\n\n")

%
% For better tracking, we will penalize more the integral variables. In particular, by only modifying the penalty over \int \delta T\left(t\right)\;\textrm{dt} as follows

Q(4,4) = 20*1/Tbar;
% New LQR controller:
KLQ = dlqr(Abar3, Bbar3, Q, R);
Kx = KLQ(:, 1:2);
Ku = KLQ(:, 3:4);
% Simulate the CL system with the new controller:
outLQi = sim("CSTR_int_ac.slx");

% we obtain:

figure
sgtitle("LQR with new Q - perturbation at t = 25s and t = 40s")
subplot(211); hold on
plot(outLQi.c)
plot(outLQi.c_ref,'-.', 'LineWidth', 1)
xlabel('t'); ylabel('\deltac(t)')
legend("linearized", "nonlinear", "reference", 'Location', 'southwest')
subplot(212); hold on
plot(outLQi.T)
plot(outLQi.T_ref,'-.', 'LineWidth', 1)
xlabel('t'); ylabel('\deltaT(t)')
legend("linearized", "nonlinear", "reference", 'Location', 'southwest')
fprintf("The reference changes at t = [0, 15, 30, 45]s.\n" + ...
    "The perturbation is added at time t = 25s and t = 40s.\n\n")

% Note that the settling time has decreased, however, the peak values have increased.