%  ---------------------------------------------------------------------  %
%   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
%
%    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 most N-rotor vehicle layouts.


% Specify the hard-coded LQR controller feedback matrix:

% > For a continuous-time controller:
K_lqr_full_state_continous_time = [...
             0         0    0.4472         0         0    0.4229         0         0         0         0         0         0   ;...
             0   -0.1414         0         0   -0.1073         0    0.3991         0         0    0.0296         0         0   ;...
        0.1414         0         0    0.1073         0         0         0    0.3991         0         0    0.0296         0   ;...
             0         0         0         0         0         0         0         0    0.2000         0         0    0.0297    ...
    ];

% > For a discrete-time controller at various frequencies:
K_lqr_full_state_discrete_time_20Hz = [...
       -0.0000   -0.0000    0.4242   -0.0000    0.0000    0.4119   -0.0000   -0.0000    0.0000   -0.0000   -0.0000    0.0000   ;...
        0.0000   -0.0727   -0.0000    0.0000   -0.0573   -0.0000    0.2215    0.0000   -0.0000    0.0215   -0.0000   -0.0000   ;...
        0.0727    0.0000   -0.0000    0.0573    0.0000   -0.0000   -0.0000    0.2215   -0.0000   -0.0000    0.0215   -0.0000   ;...
        0.0000    0.0000    0.0000    0.0000    0.0000    0.0000   -0.0000    0.0000    0.1430   -0.0000    0.0000    0.0251    ...
    ];

K_lqr_full_state_discrete_time_50Hz = [...
       -0.0000    0.0000    0.4379   -0.0000    0.0000    0.4185   -0.0000   -0.0000   -0.0000   -0.0000   -0.0000   -0.0000   ;...
       -0.0000   -0.1081   -0.0000   -0.0000   -0.0831   -0.0000    0.3137   -0.0000    0.0000    0.0260   -0.0000    0.0000   ;...
        0.1081    0.0000   -0.0000    0.0831    0.0000   -0.0000   -0.0000    0.3137    0.0000   -0.0000    0.0260    0.0000   ;...
        0.0000   -0.0000   -0.0000    0.0000   -0.0000   -0.0000    0.0000    0.0000    0.1748    0.0000    0.0000    0.0277    ...
    ];
 
K_lqr_full_state_discrete_time_100Hz = [...
        0.0000   -0.0000    0.4425    0.0000   -0.0000    0.4207    0.0000    0.0000    0.0000    0.0000    0.0000   -0.0000   ;...
        0.0000   -0.1236    0.0000    0.0000   -0.0944    0.0000    0.3536    0.0000   -0.0000    0.0277   -0.0000   -0.0000   ;...
        0.1236    0.0000    0.0000    0.0944    0.0000    0.0000   -0.0000    0.3536    0.0000   -0.0000    0.0277    0.0000   ;...
       -0.0000   -0.0000   -0.0000    0.0000    0.0000   -0.0000   -0.0000    0.0000    0.1870   -0.0000    0.0000    0.0287    ...
    ];
 
K_lqr_full_state_discrete_time_200Hz = [...
        0.0000   -0.0000    0.4449    0.0000    0.0000    0.4218   -0.0000    0.0000   -0.0000   -0.0000    0.0000    0.0000   ;...
       -0.0000   -0.1322   -0.0000   -0.0000   -0.1006   -0.0000    0.3757   -0.0000    0.0000    0.0287   -0.0000    0.0000   ;...
        0.1322    0.0000    0.0000    0.1006    0.0000    0.0000   -0.0000    0.3757    0.0000   -0.0000    0.0287    0.0000   ;...
        0.0000    0.0000    0.0000    0.0000    0.0000    0.0000    0.0000    0.0000    0.1934    0.0000    0.0000    0.0292    ...
    ];
