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

run('Ex6B.mlx');
run('Ex7B.mlx');

%
%% Point 6: Reference tracking with integral action controller
% As we have already verified that the system is reachable and observable, we now check whether the steady-state condition is fulfilled.

Sigma = [sysd.A - eye(n) sysd.B; sysd.C zeros(p, m)];
determinant = det(Sigma) 
fprintf('Since the determinant of the matrix Sigma is not zero, the \n')
fprintf('necessary steady-state condition is verified\n\n')
fprintf('----------------------------------------\n\n')

% Since the system is reachable and observable using both inputs, and since the necessary steady-state condition is satisfied, we can design an observer and a controller with integral action to achieve asymptotic perfect tracking of a constant output reference despite the presence of an unknown but constant disturbance acting on the plant states. To do so, we first design a compensator R_1, with internal state v(k), that consists of p integrators. Recall that p is the dimension of the output.

% Step 1: design of R1 with p integrators
R1.A = eye(p);
R1.B = eye(p);
R1.C = eye(p);
R1.D = zeros(p, p);
R1.x0 = zeros(p, 1);

%
% Then, we design a compensator R_2 to stabilize the augmented closed-loop system. Recall that the dynamics of the augmented state \eta \left(k\right)=\;\left\lbrack \begin{array}{c}
% x\left(k\right)\\
% v\left(k\right)
% \end{array}\right\rbrackare given by \eta \left(k+1\right)=\left\lbrack \begin{array}{cc}
% A & 0\\
% -C & I
% \end{array}\right\rbrack \eta \left(k\right)+\left\lbrack \begin{array}{c}
% B\\
% 0
% \end{array}\right\rbrack u\left(k\right)\ldotp Also, note that since \Sigma is not singular, reachability and observability of this augmented system follow from reachability and observability of the original system.

% Step 2a: define the matrices that characterize the augmented system
Ad_bar = [sysd.A zeros(n,p); -sysd.C eye(p)];
Bd_bar = [sysd.B; zeros(p, m)];
% Step 2b: design of R2 to stabilize the augmented closed-loop system
Kd_bar_eta = place(Ad_bar, Bd_bar, exp([-1 -1.05 -2 -2.05 -3 -3.05 -4 -4.05 -5].*Ts));
Kd_bar_x = Kd_bar_eta(:, 1:n);
Kd_bar_v = Kd_bar_eta(:, n+1:end);

% The Simulink implementation of the proposed control architecture is contained in the file solution_part3.mdl. Note that we use the same state observer designed in the previous exercise session.
%
% We now simulate the closed-loop system behavior and inspect how the unknown but constant disturbances affect the state estimation error.

% Simulate the closed-loop system and analyze tracking and estimation errors
dist = 5e-4*ones(1, 7);
out = sim('solution_part3.mdl');
disp('Due to the constant disturbance acting on the states, our state \n')
fprintf('does not converge to the true value...')
asymptotic_state_est_error = out.est_error.signals.values(end, :)

% Despite the non-vanishing state estimation error, we can still achieve perfect asymptotic tracking of constant output references thanks to the integral action of the controller.

fprintf('...nevertheless, the integral action controller guarantees perfect \n')
fprintf('asymptotic tracking of constant output references\n\n')
figure
sgtitle('State estimation error - disturbance at t = 35 s')
plot(out.est_error.time, out.tracking.signals.values, 'LineWidth', 1)
grid on; grid minor;xlabel('t')
legend('Output channel 1', 'Output channel 2', 'Reference 1', 'Reference 2')