sravan/Quadcopter-Control
1
0
Fork 0
mirror of https://github.com/ME-561-W20-Quadcopter-Project/Quadcopter-Control.git synced 2025-09-02 21:53:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'sensor_model' of https://github.com/ME-561-W20-Quadcopter-Project/Quadcopter-Control into sensor_model

Browse Source
This commit is contained in:
Sravan Balaji
2020-04-16 23:08:45 -04:00
parent 9266f7a3f5 8eb9c26b18
commit 66f8dc0dbc
1 changed files with 90 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files

180
src/PlantModel.m
View File

@@ -1,90 +1,90 @@
% Clear Workspace % Clear Workspace
clear all; clear all;
close all; close all;
clc; clc;
% Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf % Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
g = 9.81; g = 9.81;
m = 0.468; m = 0.468;
Ix = 4.856E-3; Ix = 4.856E-3;
Iy = 4.856E-3; Iy = 4.856E-3;
Iz = 8.801E-3; Iz = 8.801E-3;
% State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf % State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf
% X' = Ax+Bu % X' = Ax+Bu
% y = Cx+Du % y = Cx+Du
% Inputs: % Inputs:
% U1: Total Upward Force on the quad rotor along z-axis % U1: Total Upward Force on the quad rotor along z-axis
% U2: Pitch Torque (about x-axis) % U2: Pitch Torque (about x-axis)
% U3: Roll Torque (about y-axis) % U3: Roll Torque (about y-axis)
% U4: Yaw Torque (about z-axis) % U4: Yaw Torque (about z-axis)
A = [0 0 0 1 0 0 0 0 0 0 0 0;... 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 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 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 0 -g 0 0 0 0;...
0 0 0 0 0 0 g 0 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 1 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 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 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;... 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 % Note: In paper, 1/m is in wrong spot
B = [0 0 0 0;... B = [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/m 0 0 0;... 1/m 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/Ix 0 0;... 0 1/Ix 0 0;...
0 0 1/Iy 0;... 0 0 1/Iy 0;...
0 0 0 1/Iz]; 0 0 0 1/Iz];
C = [1 0 0 0 0 0 0 0 0 0 0 0;... 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 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 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 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 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 1 0 0 0];
D = zeros(6,4); D = zeros(6,4);
sys = ss(A,B,C,D); sys = ss(A,B,C,D);
sysd = c2d(sys, 1/100); sysd = c2d(sys, 1/100);
x0 = 0; x0 = 0;
y0 = 0; y0 = 0;
z0 = 0; z0 = 0;
xdot0 = 0; xdot0 = 0;
ydot0 = 0; ydot0 = 0;
zdot0 = 0; zdot0 = 0;
%phi0 = 5*(pi/180); %phi0 = 5*(pi/180);
phi0 = 0; phi0 = 0;
%theta0 = 5*(pi/180); %theta0 = 5*(pi/180);
theta0 = 0; theta0 = 0;
psi0 = 0; psi0 = 0;
phidot0 = 0; phidot0 = 0;
thetadot0 = 0; thetadot0 = 0;
psidot0 = 0; psidot0 = 0;
initialCondition = [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0]; initialCondition = [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0];
x_d = 1; x_d = 1;
y_d = 1; y_d = 1;
z_d = 1; z_d = 1;
%% %%
%Gains %Gains
kpp = 0.4754; kpp = 0.4754;
kdp = 0.1; kdp = 0.1;
kpt = 0.4754; kpt = 0.4754;
kdt = 0.1; kdt = 0.1;
kpps = 0.0901; kpps = 0.0901;
kdps = 0.0486; kdps = 0.0486;
kpz = 4.0670; kpz = 4.0670;
kdz = 2.9031; kdz = 2.9031;