%  ---------------------------------------------------------------------  %
%   DDDD   EEEEE  FFFFF    A    U   U  L      TTTTT
%   D   D  E      F       A A   U   U  L        T
%   D   D  EEE    FFF    A   A  U   U  L        T
%   D   D  E      F      AAAAA  U   U  L        T
%   DDDD   EEEEE  F      A   A   UUU   LLLLL    T
%
%    OOO   U   U  TTTTT  EEEEE  RRRR
%   O   O  U   U    T    E      R   R
%   O   O  U   U    T    EEE    RRRR
%   O   O  U   U    T    E      R   R
%    OOO    UUU     T    EEEEE  R   R
%
%    CCCC   OOO   N   N  TTTTT  RRRR    OOO   L      L      EEEEE  RRRR
%   C      O   O  NN  N    T    R   R  O   O  L      L      E      R   R
%   C      O   O  N N N    T    RRRR   O   O  L      L      EEE    RRRR
%   C      O   O  N  NN    T    R  R   O   O  L      L      E      R  R
%    CCCC   OOO   N   N    T    R   R   OOO   LLLLL  LLLLL  EEEEE  R   R
%  ---------------------------------------------------------------------  %
%% LOAD THE HARD-CODED "DEFAULT" LINEAR QUADRATIC REGULATOR (LQR) CONTROLLER

% INFO:
% > For the purposes of the template, an LQR controller feedback matrix is
%   hard-coded and should stabilise the Crazyflie Quad-rotor vehicle.


% Specify the hard-coded LQR controller feedback matrix:

% > For a continuous-time controller:
K_lqr_outer_loop_continous_time = [...
             0         0    0.5000         0         0    0.1789         0         0         0   ;...
             0   -1.0000         0         0   -0.9507         0    4.4331         0    0.0000   ;...
        1.0000         0         0    0.9507         0         0         0    4.4331         0   ;...
             0   -0.0000         0         0   -0.0000         0    0.0000         0    1.4142   ;...
    ];

% > For a discrete-time controller at various frequencies:
K_lqr_outer_loop_discrete_time_1Hz = [...
       -0.0000   -0.0000    0.0515   -0.0000    0.0000    0.0574   -0.0000   -0.0000    0.0000   ;...
        0.0000   -0.1172   -0.0000    0.0000   -0.2084   -0.0000    1.8167    0.0000    0.0000   ;...
        0.1172    0.0000    0.0000    0.2084    0.0000    0.0000   -0.0000    1.8167   -0.0000   ;...
       -0.0000    0.0000   -0.0000   -0.0000    0.0000   -0.0000   -0.0000   -0.0000    0.7321   ;...
    ];

K_lqr_outer_loop_discrete_time_100Hz = [...
        0.0000    0.0000    0.4862    0.0000    0.0000    0.1764   -0.0000    0.0000   -0.0000   ;...
       -0.0000   -0.9781   -0.0000    0.0000   -0.9348   -0.0000    4.3819    0.0000    0.0000   ;...
        0.9781   -0.0000    0.0000    0.9348   -0.0000    0.0000    0.0000    4.3819   -0.0000   ;...
       -0.0000   -0.0000   -0.0000   -0.0000   -0.0000   -0.0000    0.0000   -0.0000    1.4042   ;...
    ];
 
K_lqr_outer_loop_discrete_time_200Hz = [...
        0.0000    0.0000    0.4931    0.0000    0.0000    0.1776   -0.0000    0.0000   -0.0000   ;...
        0.0000   -0.9890    0.0000    0.0000   -0.9427   -0.0000    4.4074    0.0000    0.0000   ;...
        0.9890   -0.0000    0.0000    0.9427   -0.0000    0.0000    0.0000    4.4074    0.0000   ;...
        0.0000    0.0000   -0.0000    0.0000    0.0000   -0.0000    0.0000    0.0000    1.4092   ;...
    ];