%% ORR - lab 4 - Discrete Linear Quadratic Regulator (DLQR)
% In this Live Script, you will see how to design a linear qudratic regulator, how to implement it and what are the potential downsides of it. We will focus solely on the discrete and infinite horizon case.
% We will use the same continous-time linear model as we did last time, when we designed MPC. This willl allow us to show why MPC is actually in some aspects better than LQR.
% Let's define the continuous-time model of longitudinal dynamics of F16 aircraft and discretize it.

% Continous state-space description
Ac = [-.0151 -60.5651 0 -32.174;
     -.0001 -1.3411 .9929 0;
     .00018 43.2541 -.86939 0;
      0      0       1      0];

%  Inputs: elevator angle [deg] and flaperon angle [deg]
Bc = [-2.516 -13.136;
     -.1689 -.2514;
     -17.251 -1.5766;
     0        0];
% Outputs: attack angle [deg] and pitch angle [deg]
Cc = [0 1 0 0;
     0 0 0 1];
Dc = [0 0;
     0 0];

sys=ss(Ac,Bc,Cc,Dc);

% Discretize the model
Ts = .05;     %Sampling time
model = c2d(sys,Ts);

% Extract the matrices of the discrete state space description
[A, B, C, D] = ssdata(model);

n = size(A,1); % number of state
m = size(B,2); % number of inputs
p = size(C,1); % number of outputs

% We will also use the same initial conditions and control limits as we had last time.

% Initial state of the system
x0 = [0;2;0;10];

umax = 15*[1 1]'; % upper limits for the inputs
umin = -15*[1 1]'; % lower limits for the inputs

%
%% DLQR design in Matlab
% As you know from the lecture videos and notes, to compute the optimal state-feedback gain matrix \mathbf{K} for DLQR, one has to solve the discrete-time algebraic Riccati equation (DARE). Luckily, there is a function in Matlab called dlqr() which solves the DARE associated with DLQR design for you. You just specify the state-space description matrices \mathbf{A} and \mathbf{B} and weight matrices \mathbf{Q} and \mathbf{R}, and dlqr() gives you the optimal state-feedback gain matrix \mathbf{K}. It is as simpe as that.
% So, let's design a DLQR for out discrete-time linear model. Again, we use the same weight matrices as we did when were desgining MPC controller.

Q = diag([0 10 0 10]); % state weight matrix
R = 0.1*eye(2); % input weight matrix

K = dlqr(A, B, Q, R);

%
% Having the gain matrix \mathbf{K}, we can simulate the response of the model to the initial conditions and check how the DLQR regultes the outputs to zero. For that, we we use the accompanying Simulink model dlqr_simul_regulation.slx. Open the Simulink model and check, how the DLQR is implemented. Notice the saturation block on the control signal.
% The following section of code runs the simulation and plots the simulated response of the model. To run it, you must be in the same folder as the Simulink model is located in.

open('dlqr_simul_regulation.slx')
% Set the saturation limits on the control signals to infinity. Remember
% DLQR is guaranteed to work only for linear models, which (among other things)
% means models without saturations.
set_param('dlqr_simul_regulation/Saturation', 'UpperLimit', 'Inf'); % use 'umax' instead of 'Inf' to switch on the saturation limits
set_param('dlqr_simul_regulation/Saturation', 'LowerLimit', '-Inf');% use 'umin' instead of '-Inf' to switch on the saturation limits

% Run the simulation
SimOut = sim('dlqr_simul_regulation', 'ReturnWorkspaceOutputs', 'on');

% Extract the simulated response from the returned data
outputs = SimOut.outputsAndControls{1}.Values.Data;
controls = SimOut.outputsAndControls{2}.Values.Data;

% Plot the response
subplot(211)
plot(SimOut.tout, outputs)
title('Outputs')
xlabel('Time [s]')
ylabel('[deg]')
grid on
axis tight

subplot(212)
stairs(SimOut.tout, controls)
title('Controls')
xlabel('Time [s]')
ylabel('[deg]')
grid on
axis tight

% Have a look at the plots and check that the DLQR succesfully regulates the outputs to zero. You can also try to switch on the saturation limits (as described in the code) and check that DLQR still works even though the contidion on linearity of the model is no longer satisfied. As you will see in the next section, this is just a matter of luck.
%
%% DLQR may fail when the model is not linear
% Let's say we want to regulate the pitch angle more agresively. Pitch angle is the second output and also the forth state of the model (have a look at the matrices \mathbf{A} and \mathbf{C}) so we increase the weight on the forth state from 10 to 100.

Q = diag([0 10 0 100]); % state weight matrix
R = 0.1*eye(2); % input weight matrix

K = dlqr(A, B, Q, R);

% Simulate the response of the Simulink model for the updated matrxi \mathbf{K}.

set_param('dlqr_simul_regulation/Saturation', 'UpperLimit', 'Inf'); % use 'umax' instead of 'Inf' to switch on the saturation limits
set_param('dlqr_simul_regulation/Saturation', 'LowerLimit', '-Inf');% use 'umin' instead of '-Inf' to switch on the saturation limits

SimOut = sim('dlqr_simul_regulation', 'ReturnWorkspaceOutputs', 'on');

outputs = SimOut.outputsAndControls{1}.Values.Data;
controls = SimOut.outputsAndControls{2}.Values.Data;

subplot(211)
plot(SimOut.tout, outputs)
title('Outputs')
xlabel('Time [s]')
ylabel('[deg]')
grid on
axis tight

subplot(212)
stairs(SimOut.tout, controls)
title('Controls')
xlabel('Time [s]')
ylabel('[deg]')
grid on
axis tight

% See that the second output trully does go to zero more quickly. Nevertheless, when the input saturation limits are applied, the model becames unstable. You can try to modify the \mathbf{Q} matrix in the Live script from the previous seminar in the same way and check the MPC works perfectly fine even though DLQR is unstable. This is one of the key differences between (D)LQR and MPC. MPC can systematically incorporate various linear constraints on the states and controls.