mirror of
https://github.com/ME-561-W20-Quadcopter-Project/Quadcopter-Control.git
synced 2025-09-03 22:13:15 +00:00
53 lines
1.4 KiB
Matlab
53 lines
1.4 KiB
Matlab
%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)
|
|
|
|
|
|
|