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

IMU Sensor Model

Browse Source 
- Remove gitignore from src folder
- Add whitespace and parameterize desired position to Plant Model
- Add IMU-like sensor model to plant model with sensor
This commit is contained in:
Sravan Balaji
2020-04-16 23:04:23 -04:00
parent 7c400b7928
commit 9266f7a3f5
4 changed files with 30 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/.gitignore vendored
View File

@@ -1,4 +0,0 @@
*.slxc
*.slx.autosave
*.asv
slprj

55
src/PlantModel.m
View File

@@ -1,19 +1,24 @@
%Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf % Clear Workspace
g=9.81; clear all;
m = .468; close all;
Ix = 4.856*10^-3; clc;
Iy = 4.856*10^-3;
Iz = 8.801*10^-3;
%State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf % Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
%X' = Ax+Bu g = 9.81;
%y = Cx+Du m = 0.468;
Ix = 4.856E-3;
Iy = 4.856E-3;
Iz = 8.801E-3;
%Inputs: % State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf
%U1: Total Upward Force on the quad rotor along z-axis % X' = Ax+Bu
%U2: Pitch Torque (about x-axis) % y = Cx+Du
%U3: Roll Torque (about y-axis)
%U4: Yaw Torque (about z-axis) % 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;... 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;...
@@ -27,7 +32,7 @@ A = [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];
%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;...
@@ -45,8 +50,8 @@ C = [1 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 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);
@@ -56,19 +61,24 @@ z0 = 0;
xdot0 = 0; xdot0 = 0;
ydot0 = 0; ydot0 = 0;
zdot0 = 0; zdot0 = 0;
phi0 = 5*(pi/180)*0; %phi0 = 5*(pi/180);
theta0 = 5*(pi/180)*0; phi0 = 0;
%theta0 = 5*(pi/180);
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;
y_d = 1;
z_d = 1;
%% %%
%Gains %Gains
kpp = 0.4754; kpp = 0.4754;
kdp = .1; kdp = 0.1;
kpt = 0.4754; kpt = 0.4754;
kdt = 0.1; kdt = 0.1;
@@ -78,8 +88,3 @@ kdps = 0.0486;
kpz = 4.0670; kpz = 4.0670;
kdz = 2.9031; kdz = 2.9031;
Q = zeros(12); Q(7:9, 7:9) = eye(3);
R = 1;
%K = lqr(A,B,Q,R);

BIN
src/PlantModelSim.slx
View File

Binary file not shown.

BIN
src/PlantModelWithSensorSim.slx Normal file
View File

Binary file not shown.