%% ME-422 Multivariable control: exercise session on Gripen - solutions
% In this set of exercises, you will learn to design different control strategies for controlling the lateral dynamics of a JAS 39 Gripen aircraft flying at an altitude of 500 meters with a speed of 730 kilometers per hour. The continuous-time linearized dynamics of the aircraft are described by the state-space equations:
% \begin{array}{l}
% \dot{x} \left(t\right)=A\;x\left(t\right)+B\;u\left(t\right),\\
% y\left(t\right)=C\;x\left(t\right),
% \end{array}
% where the matrices A, B, and Care defined in the file gripen_data.mat.
%
%% Point 1: Loading the data
% We start by cleaning the workspace, loading the data

clc; close all; clear; 
% Load data
load('gripen_data.mat')

% and instantiating a state-space model for the Gripen aircraft.

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

% From the model, we can also extract the state, input, and output dimensions.

n = size(A, 1); m = size(B, 2); p = size(C, 1);

%
%% Point 2: Discretization and open-loop simulation
% In this point, we use the Simulink file solution_part1.mdl to compare the evolution of the original continuous-time system and the discretized one. We first define the initial condition of the system.

deg2rad = 2*pi/360;
x0 = [10 deg2rad deg2rad 5*deg2rad 10*deg2rad 0 0];

% We check the stability of the continuous-time linearized model by computing the eigenvalues of the matrixA.

open_loop_ct_eig = eig(A)

% Note that the open-loop system is stable but not asymptotically stable! (There is an eigenvalue at the origin)
%
% We now discretize the system with different choices of the sampling time, using both the exact and the forward Euler methods. For each case, we check whether stability is preserved or not.

choices = [0.005 0.05 0.5];
for i = 1:length(choices)
    Ts = choices(i);
    fprintf('Discretization with sampling time: %5.3f\n', Ts)
    % Discretize the system using the exact method
    sys_em = c2d(sys, Ts, 'zoh');
    open_loop_dt_exact_eig = eig(sys_em.A)
    % Check if any of the eigenvalues of the discretized system has modulus
    % greater than 1. If this were the case, then the discretized system
    % would be unstable!
    if any(abs(open_loop_dt_exact_eig) > 1)
        fprintf('Stability is not preserved!\n')
    else
        fprintf('Stability is preserved in this case\n')
    end
    % Discretize the system using the forward Euler method
    sys_fe.A = eye(n) + Ts*sys.A;
    sys_fe.B = Ts*sys.B;
    open_loop_dt_fe_eig = eig(sys_fe.A)
    % Check if any of the eigenvalues of the discretized system has modulus
    % greater than 1. If this were the case, then the discretized system
    % would be unstable!
    if any(abs(open_loop_dt_fe_eig) > 1)
        fprintf('Stability is not preserved!\n')
    else
        fprintf('Stability is preserved in this case\n\n')
    end
    fprintf('----------------------------------------\n\n')
end

% Stability properties are not always preserved by discretizing the system dynamics using the forward Euler method!
%
% To assess the importance of preserving stability through discretization, we compare the evolution of the discrete-time systems obtained using T_s =0\ldotp 5 with the exact and forward Euler methods.
% Set the inputs to zero

u1_amp = 0 ; u2_amp = 0;

% The Simulink implementation uses a discrete-time system defined in the variable sysd.
%
% First, we pass to Simulink the discrete-time system obtained using T_s =0\ldotp 5 with the exact discretization method,

sysd = sys_em;

% we simulate the system and plot the results.

out_em = sim('solution_part1.mdl');
plot_states(out_em.open_loop_ct.Time, out_em.open_loop_ct.data, ...
    out_em.open_loop_dt.Time, out_em.open_loop_dt.data)
legend(["CT", "DT"])
sgtitle("States when using exact discretization")

%
% Second, we consider the discrete-time system obtained using T_s =0\ldotp 5 with forward Euler discretization method,

sysd = sys_fe;

% we simulate the system and plot the results.

out_em = sim('solution_part1.mdl');
plot_states(out_em.open_loop_ct.Time, out_em.open_loop_ct.data, ...
    out_em.open_loop_dt.Time, out_em.open_loop_dt.data)
legend(["CT", "DT"])
sgtitle("States when using forward Euler")
fprintf('The states of this discrete-time system quickly diverge\n')
fprintf('from the ones of the continuous-time system!')

% Note that the states of this discrete-time system quickly diverge from the ones of the continuous-time system!
%
%% Point 3: Choosing a sampling time
% We now select an appropriate value for the sampling time by inspecting the poles of the original continuous-time system.
% Let's show again the eigenvalues

open_loop_ct_eig

% The fastest poles of the original continuous-time system have real part equals to -20. An appropriate value for the sampling time is \frac{1}{10\;\textrm{abs}\left(-20\right)} . Note that this is indeed the first choice.

Ts = choices(1); sysd = c2d(sys, Ts, 'zoh');