IMU Sensor Model

- Remove gitignore from src folder
- Add whitespace and parameterize desired position to Plant Model
- Add IMU-like sensor model to plant model with sensor

src/.gitignore

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

src/PlantModel.m

@@ -1,19 +1,24 @@
-%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;
+% Clear Workspace
+clear all;
+close all;
+clc;
 
-%State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf
-%X' = Ax+Bu
-%y = Cx+Du
+% Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
+g = 9.81;
+m = 0.468;
+Ix = 4.856E-3;
+Iy = 4.856E-3;
+Iz = 8.801E-3;
 
-%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)
+% 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;...
@@ -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];
 
-%Note: In paper, 1/m is in wrong spot
+% Note: In paper, 1/m is in wrong spot
 B = [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 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)
+     0 0 0 0 0 0 0 0 1 0 0 0];
+D = zeros(6,4);
 sys = ss(A,B,C,D);
 sysd = c2d(sys, 1/100);
 
@@ -56,19 +61,24 @@ z0 = 0;
 xdot0 = 0;
 ydot0 = 0;
 zdot0 = 0;
-phi0 = 5*(pi/180)*0;
-theta0 = 5*(pi/180)*0;
+%phi0 = 5*(pi/180);
+phi0 = 0;
+%theta0 = 5*(pi/180);
+theta0 = 0;
 psi0 = 0;
 phidot0 = 0;
 thetadot0 = 0;
 psidot0 = 0;
 initialCondition = [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0];
 
+x_d = 1;
+y_d = 1;
+z_d = 1;
 
 %%
 %Gains
 kpp = 0.4754; 
-kdp = .1; 
+kdp = 0.1; 
 
 kpt = 0.4754;
 kdt = 0.1;
@@ -78,8 +88,3 @@ kdps = 0.0486;
 
 kpz = 4.0670;
 kdz = 2.9031;
-
-Q = zeros(12); Q(7:9, 7:9) = eye(3);
-R = 1;
-%K = lqr(A,B,Q,R);
-