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,9 +1,14 @@
+% Clear Workspace
+clear all;
+close all;
+clc;
+
 % Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
 g = 9.81;
-m = .468;
+m = 0.468;
-Ix = 4.856*10^-3;
+Ix = 4.856E-3;
-Iy = 4.856*10^-3;
+Iy = 4.856E-3;
-Iz = 8.801*10^-3;
+Iz = 8.801E-3;
 
 % State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf
 % X' = Ax+Bu
@@ -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]
+     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);
 sysd = c2d(sys, 1/100);
 
@@ -56,19 +61,24 @@ z0 = 0;
 xdot0 = 0;
 ydot0 = 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;
 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);
-