%% --------------------------------------------------------------------- %%
%  Modelling, Control, and Estimation of N-rotor vehicles
%
%% SYNTHESIS OF LQR CONTROLLERS
%  
%  Note:
%  LQR stands for Linear Quadratic Regulator
%
%% --------------------------------------------------------------------- %%



% Specify the vehicle mass
nrotor_vehicle_mass = 32e-3;

% Specify the sample time
controller_sample_time = 1/50;

% Specify the gravitational constant:
g = 9.81;
    
% Put the vehicle's mass into a variable with a shorter name:
m = nrotor_vehicle_mass;

% Specify that the state dimension for the outer loop, denoted "nx"
nx = 9;
% > The state order is:
%   [ x ; y ; z ; xdot ; ydot ; zdot ; roll ; pitch ; yaw ]

% Specify that the input dimension is four, denoted "nu", this
% corresponds to the total thrust and the angular rates about the
% (x,y,z) axes of the body frame.
nu = 4;
% > The input order is:
%   [total thrust ; omega_x ; omega_y ; omega_z ]

% Specify the quadratic cost matrices
% J = Sum {x'Qx + u'Ru + 2*x'Su}
Q_outer_loop = diag([1, 1, 1, 1, 1, 1, 1, 1, 1]);
R_outer_loop = diag([1, 1, 1, 1]);
S_outer_loop = zeros(nx,nu);




% Continuous-time matrices for the full state equations of motion  

% State transition matrix:
A_continuous_time  =  [...
        0 0 0   1 0 0   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 0 0   0 g 0   ;...
        0 0 0   0 0 0  -g 0 0   ;...
        0 0 0   0 0 0   0 0 0   ;...
                                         ...
        0 0 0   0 0 0   0 0 0   ;...
        0 0 0   0 0 0   0 0 0   ;...
        0 0 0   0 0 0   0 0 0   ;...
    ];



% Input matrix:
B_continuous_time  =  [...
        zeros(1,nu)         ;...
        zeros(1,nu)         ;...
        zeros(1,nu)         ;...
                             ...
        zeros(1,nu)         ;...
        zeros(1,nu)         ;...
        [1/m,zeros(1,nu-1)] ;...
                             ...
        [0,1,0,0]           ;...
        [0,0,1,0]           ;...
        [0,0,0,1]           ;...
    ];



% CONTINUOUS-TIME LQR SOLUTION

% Solve the LQR formulation
[K,~,~] = lqr( A_continuous_time , B_continuous_time , Q_outer_loop , R_outer_loop , S_outer_loop);

% Put the feedback gain matrix into the return variable
K_lqr_continuous_time = K;



% ------------------------------------------------------------------- %
% DISCRETE TIME LQR SOLUTION

% Compute the sampling time
Ts = controller_sample_time;

% Output matrix
C_continuous_time = eye(nx);

% The feed-through matrix
D_continuous_time = zeros(nx,nu);

% Convert continuous-time state space to discrete-time
nrotor_sys_continous_time   =  ss(A_continuous_time, B_continuous_time, C_continuous_time, D_continuous_time);
nrotor_sys_discrete_time    =  c2d(nrotor_sys_continous_time, Ts , 'zoh');
[A_discrete_time, B_discete_time, ~, ~] = ssdata(nrotor_sys_discrete_time);

% Solve the LQR formulation
[K,~,~] = dlqr(A_discrete_time, B_discete_time, Q_outer_loop, R_outer_loop, S_outer_loop);

% Put the feedback gain matrix into the return variable
K_lqr_discrete_time = K;
    


% Display the discrete time controller matrix
disp(K_lqr_discrete_time);