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

run('Ex6B.mlx');

%
%% Point 4: Reachability analysis and eigenvalue assignment
% We first check whether system is reachable using both inputs by computing the rank of the reachability matrix.

rank_ctrb_both_inputs = rank(ctrb(sysd.A, sysd.B))
fprintf('Since the rank of the reachability matrix equals the system\n')
fprintf('dimension, the system is reachable using both inputs\n\n')

% Since the system is reachable using both inputs, we can assign the eigenvalues of the closed-loop system arbitrarily using a state-feedback controller.

% Design two state-feedback controllers to assign the closed-loop poles as
% requested
p1 = exp([-0.1 -0.1 -1 -2 -3 -5 -5]*Ts); % Slower dynamics
K1d = place(sysd.A, sysd.B, p1);

p2 = exp([-1 -2 -3 -4 -5 -20 -30]*Ts);   % Faster dynamics
K2d = place(sysd.A, sysd.B, p2);

% We then simulate the closed-loop system using these two state-feedback controllers and compare the input signals that these controllers generate.

out = sim('solution_part2a.mdl');
figure
sgtitle('Simulation of the closed-loop system with different state-feedback controllers')
subplot(2, 1, 1)
plot(out.cl_dt_Kd1.time, reshape(out.cl_dt_Kd1.signals.values(1, :, :), length(out.cl_dt_Kd1.time), 1), 'LineWidth', 1)
hold on; grid on; grid minor;
plot(out.cl_dt_Kd2.time, reshape(out.cl_dt_Kd2.signals.values(1, :, :), length(out.cl_dt_Kd2.time), 1), 'LineWidth', 1)
legend('Input 1: state-feedback controller 1', 'Input 1: state-feedback controller 2') 
subplot(2, 1, 2)
plot(out.cl_dt_Kd1.time, reshape(out.cl_dt_Kd1.signals.values(2, :, :), length(out.cl_dt_Kd1.time), 1), 'LineWidth', 1)
hold on; grid on; grid minor;
plot(out.cl_dt_Kd2.time, reshape(out.cl_dt_Kd2.signals.values(2, :, :), length(out.cl_dt_Kd2.time), 1), 'LineWidth', 1)
legend('Input 2: state-feedback controller 1', 'Input 2: state-feedback controller 2') 

% Note that faster convergence to the origin comes at the price of more aggressive control actions!
%
% We now verify if the system is also reachable using only a single control input. To do so, we compute the rank of the reachability matrix obtained by using only the first or the second column of the matrix B\ldotp

rank_ctrb_first_input = rank(ctrb(sysd.A, sysd.B(:, 1)))
fprintf('The system is not reachable using the first input channel only\n\n')
rank_ctrb_second_input = rank(ctrb(sysd.A, sysd.B(:, 2)))
fprintf('The system is not reachable using the second input channel only\n\n')
fprintf('----------------------------------------\n\n')

% Since the system is not reachable using a single input channel, not all the closed-loop system eigenvalues can be arbitrarily assigned using a state-feedback controller. To check whether the pole placement problem can be solved, we first need to isolate the non-controllable subspace (dimension: 7 - 6 = 1).

% Compute a change of coordinates to isolate the non-controllable subspace
T = inv([orth(ctrb(sysd.A, sysd.B(:, 2))) null(ctrb(sysd.A, sysd.B(:, 2))')]);
% Compute the matrices associated with this new state-space representation,
% i.e. the matrices that describe the dynamics of the new state x_hat = T*x
Ad_hat = T*sysd.A*inv(T); 
Bd_hat = T*sysd.B(:, 2);

% We now check which is the uncontrollable eigenvalue of the system that can not be freely assigned using only the second input channel.

Ad_hat

% Since the uncontrollable eigenvalue is 0\ldotp 9048=e^{-20T_s }, it is possible to assign the closed-loop eigenvalues as in point 4b using the second input channel only (but it is not possible to assign them as in point 4a!). Leveraging the controllable subspace only, we finally assign the remaining closed-loop eigenvalues, and then express the obtained controller back in the original coordinates.

% Assign the remaining closed-loop eigenvalues
K3d_hat = place(Ad_hat(1:6, 1:6), Bd_hat(1:6), exp([-1 -2 -3 -4 -5 -30]*Ts));
% Get the corresponding feedback controller in the original x coordinates 
K3d = [K3d_hat 0]*T;
% Verify that the pole placement problem has been correctly solved
p2
p3 = eig(sysd.A - sysd.B(:, 2)*K3d)'

% Up to a permutation, all the eigenvalues are the same.
%
%% Point 5: Observability analysis and design of Luenberger observers
% We first check whether the system is observable by computing the rank of the observability matrix.

rank_obs_both_outputs = rank(ctrb(sysd.A', sysd.C'))
fprintf('Since the rank of the observability matrix equals the system\n')
fprintf('dimension, the system is observable using both outputs\n\n')

% Since the system is observable, we can design a Luenberger observer to asymptotically reconstruct the state vector.

% Design an observer gain to assign the eigenvalues of the error dynamics
Ld = place(sysd.A', sysd.C',exp([-10 -10 -20 -20 -30 -30 -40].*Ts))'; % Faster than the controller

% Recall that the Luenberger observer takes u\left(t\right) and y\left(t\right) as inputs and it outputs a state estimate \hat{x} \left(t\right). We now define the observer in state-space form and set its initial condition. In absence of any information, this can be chosen arbitrarily.

obsd.A = sysd.A - Ld*sysd.C;
obsd.B = [sysd.B - Ld*sysd.D Ld];
obsd.C = eye(7);
obsd.D = zeros(7, 4);
% Set an initial condition for the Luenberger observer
obsd.x0 = zeros(7, 1);

% We now compute the asymptotic estimation error caused by constant output measurement disturbances and we validate the result through simulation. Note that if y\left(t\right)=C\;x\left(t\right)+d then:
% \hat{x} \left(t+1\right)=A\;\hat{x} \left(t\right)+B\;u\left(t\right)+L\left(C\;x\left(t\right)+d-C\;\hat{x} \left(t\right)\right)\; and e\left(t+1\right)=\left(A-L\;C\right)\;e\left(t\right)-L\;d.
% where e\left(t\right)=x\left(t\right)-\hat{x} \left(t\right).
% Since at steady state e\left(t+1\right)=e\left(t\right)=e, we then have that \left(I-\left(A-L\;C\right)\right)e=-L\;d, and, therefore, e=-{\left(I-A_{\textrm{obs}} \right)}^{-1} L\;d.

dist = [-0.2; 0.2]; % active for t>4
theoretical_est_error = (-inv(eye(7) - obsd.A)*Ld*dist)'
out = sim('solution_part2c.mdl');
simulated_est_error = out.est_error.signals.values(end, :)

% We compare the evolution of the true and estimated states when a perturbation occurs.

plot_states(out.sys.time, out.sys.signals.values, ...
    out.est.time, out.est.signals.values)
legend(["True state", "Estimated state"])
sgtitle("True and estimated states -- perturbation at t = 4s")

%
% Finally, we consider the design of a reduced-order observer. To do so, we start by computing a change of coordinates that makes the first two states coincide with the two outputs.

To = [0 0 0 1 0 0 0; ...
      0 0 0 0 1 0 0; ...
      1 0 0 0 0 0 0; ... 
      0 1 0 0 0 0 0; ...
      0 0 1 0 0 0 0; ...
      0 0 0 0 0 1 0; ...
      0 0 0 0 0 0 1];

% We then compute the matrices associated with this new state-space representation, namely, the matrices that describe the dynamics of the new state \bar{x\;} =T\;x=\left\lbrack y;w\right\rbrack.

Ad_bar = To*sysd.A*inv(To);
Bd_bar = To*sysd.B;
Cd_bar = sysd.C*inv(To);
% Partition the matrices Ad_bar and Bd_bar according to x_bar = To*x = [y; w]
Ad_bar11 = Ad_bar(1:2, 1:2);
Ad_bar12 = Ad_bar(1:2, 3:7);
Ad_bar21 = Ad_bar(3:7, 1:2);
Ad_bar22 = Ad_bar(3:7, 3:7);
Bd_bar1 = Bd_bar(1:2, :);
Bd_bar2 = Bd_bar(3:7, :);

% Next, we design the reduced-order obsever. Recall that this takes u\left(t-1\right), y\left(t-1\right) and y\left(t\right) as inputs, and that it outputs a state estimate \hat{w} \left(t\right), while keeping \hat{w} \left(t-1\right) as internal state.
% \begin{array}{l}
% \hat{w} \left(t\right)={\bar{A} }_{22} \;\;\hat{w} \left(t-1\right)+{\bar{A} }_{21\;} y\left(t-1\right)+{\bar{B} }_2 \;u\left(t-1\right)+L\;\left(\;y\left(t\right)\;-{\;\bar{A} }_{11} \;y\left(t-1\right)-{\bar{B} }_{1\;} u\left(t-1\right)-{\bar{A} }_{12} \;\hat{w} \left(t-1\right)\right)\\
% \;\;\;\;\;\;\;\;\;=\left({\bar{A} }_{22} -L\;{\bar{A} }_{12} \right)\;\hat{w} \left(t-1\right)+\left({\bar{B} }_2 -L\;{\bar{B} }_{1\;} \right)\;u\left(t-1\right)+\left({\bar{A} }_{21\;} -L\;{\;\bar{A} }_{11} \right)\;y\left(t-1\right)+L\;y\left(t\right)
% \end{array}

% Design the reduced order observer gain matrix
Ld_red = place(Ad_bar22', Ad_bar12', exp([-10 -10 -20 -20 -25].*Ts))';
% Define the state-space matrices of the reduced order observer: 
% inputs: system inputs (u(t-1)) and system outputs (y(t-1), y(t))
% state: estimate w_hat(t-1)
% outputs: state estimate (w_hat(t))
obsd_red.A = Ad_bar22 - Ld_red*Ad_bar12;
obsd_red.B = [Bd_bar2 - Ld_red*Bd_bar1 Ad_bar21 - Ld_red*Ad_bar11 Ld_red];
obsd_red.C = obsd_red.A;
obsd_red.D = obsd_red.B;
% Set an initial condition for the reduced order observer
obsd_red.x0 = zeros(5, 1);
%%% If you wish to validate your design you can always initialize the
%%% reduced-order observer with the true initial condition of the system.
%%% In this case, you should see that the state estimate coincides with the
%%% true state at all times.
x0obs = x0*2;
tmp = To*x0obs'; tmp(1:2) = []; obsd_red.x0 = inv(obsd_red.A)*(tmp - Ld_red*sysd.C*x0');

% Finally, we verify the reduced order observer design by simulation by plotting the evolution of the true and estimated states.

out_red = sim('solution_part2d.mdl');
% Plot true and estimated states
plot_states(out_red.sys.time, out_red.sys.signals.values, ...
    out_red.est_red.time, out_red.est_red.signals.values)
legend(["True state", "Estimated state"])
sgtitle("True and estimated states using a reduced order observer")