%% 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-6: Load data from previous exercise sessions

run('Ex8B.mlx');

%
%% Point 7: Linear quadratic regulator control design
% Remember that we have already verified that the system is reachable. Hence, for any Q\succeq 0 and R\succ 0, the discrete-time algebraic Riccati equation has a solution \bar{P} \succeq 0. We now design the weight matrices for the linear quadratic regulator control design using the normalization approach.

q = 0.5; r = 0.5; % These values can be chosen arbitrarily in the interval (0, 1)
% Construct the diagonal state weight matrix Q
q1 = q/(12)^2;
q2 = q/(5*pi/36)^2; q3 = q2;
q4 = q/(pi/3)^2; q5 = q4;
q6 = q/(13*pi/36)^2; q7 = q6;
Q = diag([q1,q2,q3,q4,q5,q6,q7]);
% Construct the diagonal input weight matrix R
r1 = r/(pi/6)^2; r2 = r1;
R = diag([r1,r2]);

% Then, we proceed to compute the discrete-time inifnite horizon linear quadratic regulator.

% Compute the discrete-time inifnite horizon linear quadratic regulator
Kd_lqr = dlqr(sysd.A, sysd.B, Q, R);

% To verify that the optimal feedback gain is stabilizing, we can compute the eigenvalues of the closed-loop system.

closed_loop_eig = eig(sysd.A - sysd.B*Kd_lqr)
closed_loop_eig_modulus = abs(closed_loop_eig)
fprintf('All closed-loop eigenvalues lie inside the unit circle\n\n')

% As seen in the lectures, since the pair \left(A,B\right) is reachable, one can also deduce that the optimal feedback gain is stabilizing from the observability of the pair \left(A,C_1 \right), where C_1 is such that Q=C_1^T \;C_1.

C1 = sqrtm(Q);
if rank(ctrb(sysd.A', C1')) == n
    fprintf('The pair (A, C1) is observable and therefore the optimal\n')
    fprintf('feedback gain is stabilizing\n\n')
end

%
%% Point 8: Kalman filter design
% To design a minimum variance Kalman predictor, we first define the covariance matrices that characterize the disturance processes acting on the system states and outputs.

% Define the covariance matrix of the process and measurement noises
W = 1e-04*diag([12, 5*pi/36, 5*pi/36, pi/3, pi/3, 13*pi/36, 13*pi/36]);
V = 1e-04*eye(2);

% Remember that we have already verified that the system is observable. We now check that the pair \left(A,B_1 \right), where B_1 is such that W=B_1 B_1^T, is reachable so that existence and stability of the optimal steady-state predictor is guaranteed.

B1 = sqrtm(W);
if rank(ctrb(sysd.A, B1)) == n
    fprintf('The pair (A, B1) is reachable and therefore the optimal\n')
    fprintf('steady-state predictor is stabilizing\n\n')
end

% Then, we compute the unique positive definition solution of the discrete-time algebraic Riccati equation, from which  we can construct the Kalman filter optimal gain.

% Solve the discrete-time algebraic Riccati equation
Sigma = idare(sysd.A', sysd.C', W, V);
% Compute the optimal gain for the Kalman filter
L = sysd.A*Sigma*sysd.C'*inv(sysd.C*Sigma*sysd.C' + V);

%
% We construct a state-space description of the optimal steady-state predictor, which we initialize at zero as we have already done for the Luenberger observer.

% Construct the matrices that describe the dynamics of the Kalman filter
obsd_kalman.A = sysd.A - L*sysd.C;
obsd_kalman.B = [sysd.B L];
obsd_kalman.C = eye(n);
obsd_kalman.D = zeros(n, m+p);
% Set an initial condition for the Kalman filter
obsd_kalman.x0 = zeros(n, 1);

% As a double check, we verify that the optimal predictor stabilizes the error dynamics.

% Verify that the designed Kalman filter stabilizes the error dynamics
kalman_filter_eig = eig(obsd_kalman.A)
kalman_filter_eig_modulus = abs(kalman_filter_eig)
fprintf('All Kalman filter eigenvalues lie inside the unit circle\n\n')

% We now compare the performance of the Kalman filter and the Luenberger observer designed in a previous exercise session when the system is affected by stochastic disturbances.

% Simulate the behavior of the Kalman filter and Luenberger observer
out = sim('solution_part4a.mdl');
figure
sgtitle('State estimation error using Kalman filter and Luenberger observer')
for i = 1:n
    subplot(7, 1, i)
    plot(out.est_error_kalman.time, out.est_error_kalman.signals.values(i, :), 'LineWidth', 1)
    hold on; grid on; grid minor;
    plot(out.est_error_luenberger.time, out.est_error_luenberger.signals.values(i, :), 'LineWidth', 1)
end
legend('Kalman filter', 'Luenberger observer')

% We then verify that the innovation sequence is zero-mean.

innovation_seq_output_1 = mean(out.innovation.signals.values(1, :))
innovation_seq_output_2 = mean(out.innovation.signals.values(2, :))

%
% Finally, we close the loop and simulate the behavior of the control scheme obtained by combining the linear quadratic optimal feedback gain with the Kalman filter.

out = sim('solution_part4b.mdl');
figure
hold off
sgtitle('Closed-loop simulation: Kalman filter and linear quadratic controller')
for i = 1:n
    subplot(7, 1, i)
    plot(out.true_states.time, out.true_states.signals.values(i, :), 'LineWidth', 1)
end