%% 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-5: Load data from previous exercise session

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

%
%% Point 6: Discretization
% We discretize the linearized system around \left(\bar{c} ,\bar{T} \right) with inputs \left({\bar{c} }_f ,{\bar{T} }_c \right). We set the sampling time as per:

Ts = 0.05;

% Using Matlab, we can discretize the system by defining the continuous-time state-space model

sys = ss(A,B,C,D);

% and converting it into a discrete-time model. For this, we use the c2d command that uses, by default, the exact discretization method ('zoh', i.e. zero-order hold).

sysd = c2d(sys, Ts);

% Then, the matrices characterizing the linear discretized system are

Ad = sysd.A; Bd = sysd.B; Cd = sysd.C; Dd = sysd.D;

%
% For simulating the linearized system around \left(\bar{c} ,\bar{T} \right) both in continuous and discrete time, we construct a new Simulink model: CSTR_discretization.slx. Note that we add a ‘zero-order hold’ block with sampling time Ts at the input of the discrete-time system model. We plot the state trajectories.

% Simulate the CT and DT from some initial condition
x0 = 0.01 * [cbar ; Tbar];
out_dt = sim("CSTR_discretization");
figure
subplot(121)
plot(out_dt.y_ct.time,out_dt.y_ct.data(:,1), 'LineWidth', 1); hold on
stairs(out_dt.y_dt.time,out_dt.y_dt.data(:,1), 'LineWidth', 1); hold off
legend("\deltac_{CT}(t)", "\deltac_{DT}(t)"); title("Concentration"); xlabel('t')
subplot(122)
plot(out_dt.y_ct.time,out_dt.y_ct.data(:,2), 'LineWidth', 1); hold on
stairs(out_dt.y_dt.time,out_dt.y_dt.data(:,2), 'LineWidth', 1); hold off
legend("\deltaT_{CT}(t)", "\deltaT_{DT}(t)"); title("Temperature"); xlabel('t')

% Note that at the sampling times t_k =kT_s, for k=0,1,2,\ldotp \ldotp \ldotp, we have that {\delta c}_{\textrm{CT}} \left(t_k \right)={\delta c}_{\textrm{DT}} \left(t_k \right) and {\delta T}_{\textrm{CT}} \left(t_k \right)={\delta T}_{\textrm{DT}} \left(t_k \right). This is expected since we used the exact discretization method.
% Moreover, note that the physical meaning of the concentration is lost when \delta c drops below -0\ldotp 5\;\textrm{mol}, since the concentration must be a positive number.
%
%% Point 7a: controller design
% We design a controller for assigning the two closed-loop eigenvalues at 0.85 using pole placement.
% Firstly, we check if the linearized system is reachable

rank_ctrb = rank(ctrb(Ad,Bd))
disp("The system is reachable.")

% Since the rank of the reachability matrix equals the system dimension, the system is reachable.
% Then, the eigenvalue assignment problem can be solved!
% We compute the regulator Kp and the closed-loop evolution matrix

Kp = place(Ad,Bd,[0.85 0.85]);
Acl_p = Ad-Bd*Kp

%
%% Point 7b: Simulink model
% We construct the closed-loop system when using the original non-linear system and its linearized version around \left(\bar{c} ,\bar{T} \right) in the CSTR_poles.slx file.
% Recall that the linearized model states are \delta c and \delta T, and its inputs \delta c_f and \delta T_c. Since the controller has been designed for this linearized system, in the nonlinear case, we need to substract (add) the state (input) values before (after) the regulator block.
%
%% Point 7c: Simulation of the closed-loop system
% We consider perturbed initial conditions of +1% and +4% around \left(\bar{c} ,\bar{T} \right) and we simulate the systems.

x0 = 0.01 * [cbar ; Tbar]; % Small perturbation
outp(1) = sim("CSTR_poles.slx");
x0 = 0.04 * [cbar ; Tbar]; % Increasing the perturbation
outp(2) = sim("CSTR_poles.slx");

% Finally, we plot the results

for i=1:2
figure
title_ = sprintf("Comparison between linearized and nonlinear CL " + ...
    "behavior for %i%% perturbation", (i-1)*3+1);
sgtitle(title_)
subplot(211)
plot(outp(i).delta_c, 'LineWidth', 1)
xlabel("t"); ylabel("\deltac(t)"); title("output: c")
legend("linearized", "nonlinear")
subplot(212)
plot(outp(i).delta_T, 'LineWidth', 1)
xlabel("t"); ylabel("\deltaT(t)"); title("output: T")
legend("linearized", "nonlinear")
end
fprintf("The stabilizing controller for the linearized model do not guarantee\n" + ...
    "the stability of the original nonlinear model.\n\n")

% Note that the controller has been designed for the linearized model. Thus, it is expected to asymptotically stabilize the linearized system, i.e. to drive the blue curve to zero for any intial condition.
% However, asymptotic stability of the closed-loop sytem when using the nonlinear model is not guaranteed, as it may depend on the initial condition.
%
% Recall that the state variables do not converge to zero!
% Indeed, the state trajecotories of the nonlinear system are:

figure
sgtitle("Nonlinear system state evolution for 1% perturbation")
subplot(211)
plot(outp(1).cnl, 'LineWidth', 1)
%hold on; plot(outp(2).cnl)
xlabel("t"); ylabel("c(t)"); title("output: c")
%legend("1% perturbation", "4% perturbation")
subplot(212)
plot(outp(1).Tnl, 'LineWidth', 1) 
%hold on; plot(outp(2).Tnl)
xlabel("t"); ylabel("T(t)"); title("output: T")
%legend("1% perturbation", "4% perturbation")

%
%% Point 7d: ROA
% For obtaining the region of attraction (ROA), we sample the initial conditions in the asked set, i.e., such that \left|c-\bar{c} \right|<0\ldotp 25\;\frac{\textrm{mol}}{\mathrm{L}} and \left|T-\bar{T} \right|<25\;\mathrm{K}. We consider two grids, one coarse, where we consider 25 initial conditions, and one fine where 625 initial conditions are considered.
% By uncommneting the second line, one can switch between the two cases.

gr = 5; % coarse grid
% gr = 25; % fine grid

% We define the initial conditions to be considered

grid_c = linspace(-0.25,0.25,gr);
grid_T = linspace(-25,25,gr);
grid_ROA = zeros(length(grid_c), length(grid_T));

% Then, we simulate the system and check whether the states converge approximately to zero.

for i=1:length(grid_c)
    for j=1:length(grid_T)
        x0 = [grid_c(i); grid_T(j)];
        outi = sim("CSTR_poles.slx");
        if (abs(outi.delta_c.data(end,2))<1e-5) && (abs(outi.delta_T.data(end,2))<1e-1)
            grid_ROA(i,j) = 1;
        end
    end
end

% Finally, we plot the ROA in the c_0-T_0 plane.

figure; hold on;
for i=1:length(grid_c)
    for j=1:length(grid_T)
        if grid_ROA(i,j) == 1
            color_plot = 'r';
        else
            color_plot = 'b';
        end
        plot(grid_c(i)+cbar,grid_T(j)+Tbar, color_plot, ...
            'Marker',"square","MarkerSize",15,'MarkerFaceColor',color_plot)
    end
end
%contourf(grid_c+cbar, grid_T+Tbar, grid_ROA)
hold on; plot(cbar,Tbar,'.','MarkerSize',20, 'color', 'g'); hold off
xlabel("c_0"); ylabel("T_0")
title("ROA - Blue indicates instability of x_0 = [c_0 , T_0]")

%  In blue, we inidicate the initial conditions that make the system unstable while yellow indicates the initial conditions for which the system remains stable.
%
%% Point 8: Luenberger observer
% Assuming now that only the second state (T\left(t\right)) can be measured directly, we design an observer for estimating c\left(t\right). For this, we start by redefining the matrices Cd and Dd.

Cd_2 = Cd(2, :); Dd_2 = Dd(2, :); 

% Then, we check whether the system is observable from the second output channel

rank_obs = rank(obsv(Ad, Cd_2))
disp("The system is observable")

% Since the rank of the observability matrix equals the system dimension, the system is observable when considering only the second output.
% Thus, we design an observer to assign the eigenvalues of the error dynamics at 0.5 and 0.55.

Ld = place(Ad', Cd_2', [0.5 0.55])';

% The dynamics of the Luenberger observer are defined with:
% inputs: system inputs and outputs
% outputs: the state estimate

obsd.A = Ad - Ld*Cd_2;
obsd.B = [Bd - Ld*Dd_2 Ld];
obsd.C = eye(2);
obsd.D = zeros(2, 3);

% We set an initial condition for the Luenberger observer

obsd.x0 = zeros(2, 1);

% and a different initial condition for the linearized discrete-time system

x0 = 0.03*[cbar; Tbar];

% Then, we simulate the closed-loop system with the controller of the previous point. For this, we generate a new model in CSTR_pp_obs.slx where:
% we consider the system with only one output (T\left(t\right)),
% we add the observer modeled by obsd,
% we keep the same controller as before defined by Kp.

out_obs = sim("CSTR_pp_obs.slx");

% Finally, we plot the true state of the linearized system, the estimated state obtained by using the Luenberger observer, and the true state given by the nonlinear model.

figure
subplot(211)
plot(out_obs.delta_c, 'LineWidth', 1)
xlabel("t"); ylabel("\deltac(t)"); title("unmeasured output: c")
legend("'true' state - linear", "estimated state - linear", "nonlinear")
subplot(212)
plot(out_obs.delta_T, 'LineWidth', 1)
xlabel("t"); ylabel("T(t)"); title("measured output: T")
legend("'true' state - linear", "estimated state - linear", "nonlinear")

%
% We also show that indeed the error between the estimated ({\delta c}_e \left(t\right) and {\delta T}_e \left(t\right)) and the true states (\delta c\left(t\right) and \delta T\left(t\right)) does go to zero both for the linearized system and the nonlinear model.

figure
sgtitle("Estimation errors")
subplot(211)
plot(out_obs.error_c, 'LineWidth', 1)
xlabel("t"); ylabel("\deltac(t) - \deltac_{e}(t)"); title("output: error in \deltac")
legend("linearized", "nonlinear")
subplot(212)
plot(out_obs.error_T, 'LineWidth', 1)
xlabel("t"); ylabel("\deltaT(t) - \deltaT_{e}(t)"); title("output: error in \deltaT")
legend("linearized", "nonlinear")