%% 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');
n = size(sysd.A,1);
m = size(sysd.B,2);

%
%% 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')
sysd.A
sysd.B

%
%% Point 9: Distributed Linear regulator design: no v_y measurements
% The ground speed v is measured by firing a radar beam towards the ground and measuring the Doppler shift of the returning beam. This ground speed measurement is essential in correctly controlling the Grifen. However, the sensor has broken and you have to make do with the other available state measurements. Suppose that
%

A = sysd.A;
B = sysd.B;
n = size(A,1);
m = size(B,2);
Sigma_0 = eye(n);

% Your controller must now comply with a sparsity pattern
% S=\;\left\lbrack \begin{array}{ccccccc}
% 0 & 1 & 1 & 1 & 1 & 1 & 1\\
% 0 & 1 & 1 & 1 & 1 & 1 & 1
% \end{array}\right\rbrack

S = [0 1 1 1 1 1 1; 0 1 1 1 1 1 1];

% In order to cope with the fact that v_y cannot be measured anymore, we increase the cost weight Q(1,1)

Q(1,1) = 10 * Q(1,1);

% The LQR controller becomes

P = idare(A,B,Q,R,zeros(n,m),eye(n));
K_LQR = inv(B'*P*B+R)*B'*P*A

% which does not comply with the sparsity pattern above, and has a cost

cost_LQR = trace(P * Sigma_0)

% The first idea is to preserve the LQR matrix as much as possible. Hence, we try to use the distributed controller

K0 = K_LQR .* S;
K = K0

% which is stabilizing and has a cost

cost_K0 = trace(dlyap((A-B*K0)',Q+K0'*R*K0)*Sigma_0)

% It is now time to apply projected gradient descent to improve the choice of K0. We choose a stepsize \eta=0.000001, initialize the gradient norm and a counter of iterations, and choose a tolerance level \epsilon= 0.01 to stop the iterations:

eta = 0.000001; %step-size
gradient_norm = 10000; %stopping 
count = 0;

while(gradient_norm>0.01 )
   
    count = count+1;
    
    DeltaJ = eval_gradient(A,B,K,Sigma_0,Q,R);
    DeltaJ_projected = DeltaJ .* S;
    
    gradient_norm = sum(sum(DeltaJ_projected .* DeltaJ_projected));
    
    if(mod(count,10000)==0)
        cost_K = eval_cost(A,B,K,Q,R,Sigma_0);
        fprintf('Iteration: %d, Cost: %f\n', count, cost_K)
        fprintf('gradient norm: %d\n\n\n', gradient_norm)
        eig(A-B*K)'
    end

    K = K-eta * DeltaJ_projected;
    K = K .* S;
end


% Time for some plots

plots

% The new controller almost matches the LQR performance, while the initial K0 is way off. We managed to control well the lateral velocity... without ever measuring it!