% QuadrotorLQRIinitialize.m
%
% Initialize code for simulation of baseline LQR controller + integral action 
% for a quadrotor 
%
% Reference:
% D. Xu, J.F. Whidborne A. Cooke ,
% "Fault tolerant control of a quadrotor using $L_1$ adaptive control"
% International Journal of Intelligent Unmanned Systems, (2016),
% Vol. 4 Iss 1 pp. 43 - 66
% http://dx.doi.org/10.1108/IJIUS-08-2015-0011

% (c) Dan Xu, James F Whidborne
%     Cranfield University 2015


%% define quadrotor constants
% mass, inertias & geometry (eqs 14 & 15)
m=2.356;     % mass (kg)
Ixx=0.1676;  % I_x (kgm^s)
Iyy=0.1676;  % I_y (kgm^s)
Izz=0.29743; % I_z (kgm^s)
g=9.81;      % gravitational constant (ms^-2)
larm=0.5;    % arm length (m)
% motors & rotors (eqs 16 & 17)
Vmax=14.8;  % max voltage
kf=0.11;    % thrust constant f = k_f*v^2 (eq 16)\approx g/90
ktau=0.052; % torque constant tau = ktau*v^2 (eq 17) 

%% linearized open loop system (eqs 45 - 47)
% state variables = \delta  [U ,V ,W ,P ,Q ,R ,X ,Y ,Z ,\phi ,\theta , \psi]
A=  [0 0 0     0 0 0    0 0 0    0 -g 0;...
    0 0 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;...
    0 0 0     0 0 0    0 0 0    0 0 0;...
    1 0 0     0 0 0    0 0 0    0 0 0;...
    0 1 0     0 0 0    0 0 0    0 0 0;...
    0 0 1     0 0 0    0 0 0    0 0 0;...
    0 0 0     1 0 0    0 0 0    0 0 0;...
    0 0 0     0 1 0    0 0 0    0 0 0;...
    0 0 0     0 0 1    0 0 0    0 0 0];
b=[  0     0     0     0;...
    0     0     0     0;...
    -1/m    0     0     0;...
    0    1/Ixx  0     0;...
    0     0   1/Iyy   0;...
    0     0     0 1/Izz;...
    0     0     0     0;...
    0     0     0     0;...
    0     0     0     0;...
    0     0     0     0;...
    0     0     0     0;...
    0     0     0     0];
C = [0 0 0     0 0 0    1 0 0    0 0 0;...
    0 0 0     0 0 0    0 1 0    0 0 0;...
    0 0 0     0 0 0    0 0 1    0 0 0;...
    0 0 0     0 0 0    0 0 0    0 0 1];
D=zeros(4,4); 
x_initial=[0;0;0;0;0;0;0;0;0;0;0;0];
 
%% control allocation matrix (eq 19)
Lambda= [
    kf         kf        kf         kf;...
    0         -larm*kf   0          larm*kf;...
    larm*kf    0        -larm*kf    0;...
    ktau      -ktau      ktau      -ktau];
B=b*Lambda;
v_trim=m*g/4/kf;                          % trim voltage (eq 49)
V_trim=[v_trim; v_trim; v_trim; v_trim];  % trim voltage vector
V_stauration=14.8^2;

%% LQR controller design - altitude and attitude (section 4.3)
% augment plant (eq 60)
Aaug=[A, zeros(12,4);
      -C, zeros(4,4)];
Baug=[B;zeros(4,4)];
 
%weighting matrices
Q=diag([100*ones(1,12),1000*ones(1,4)]) ; % gives similar bandwidth to L1 adaptive controller
R=eye(4); 
Kaug = lqr(Aaug,Baug,Q,R); % LQR controller
%  partition LQR controller (eq 62)
Klqrp=Kaug(:,1:12);        % state feedback K_p 
Klqri=Kaug(:,13:end);      % integral action K_i
Kg=eye(4);                 % tracking pre-compensator K_g

%% define failure level 
vfail=sqrt(0.35)