%Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
g=9.81;
m = .468;
Ix = 4.856*10^-3;
Iy = 4.856*10^-3;
Iz = 8.801*10^-3;

%State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf
%X' = Ax+Bu
%y = Cx+Du

%Inputs:
%U1: Total Upward Force on the quad rotor along z-axis
%U2: Pitch Torque (about x-axis)
%U3: Roll Torque (about y-axis)
%U4: Yaw Torque (about z-axis)
A = [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  -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  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  0  0  0  0  0  0  0  0  0  0;...
     0  0  0  0  0  0  0  0  0  0  0  0];

%Note: In paper, 1/m is in wrong spot
B = [0 0 0 0;...
     0 0 0 0;...
     0 0 0 0;...
     0 0 0 0;...
     0 0 0 0;...
     1/m 0 0 0;...
     0 0 0 0;...
     0 0 0 0;...
     0 0 0 0;...
     0 1/Ix 0 0;...
     0 0 1/Iy 0;...
     0 0 0 1/Iz];
C = [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 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]
D = zeros(6,4)