Added PD controller

src/.gitignore

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

src/PlantModel.m

@@ -40,12 +40,38 @@ B = [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];
+     0 0 0 0 0 0 0 0 1 0 0 0]
+D = zeros(6,4)
+
+x0 = 0;
+y0 = 0;
+z0 = 0;
+xdot0 = 0;
+ydot0 = 0;
+zdot0 = 0;
+phi0 = 10*(pi/180);
+theta0 = 10*(pi/180);
+psi0 = 0;
+phidot0 = 0;
+thetadot0 = 0;
+psidot0 = 0;
+initialCondition = [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0];
+
+%Gains
+kpp = 10;
+kdp = 0.2;
+
+kpt = 4;
+kdt = 0.2;
+
+kpps = 1;
+kdps = .4;
+
+kpz = 100;
+kdz = 20;
 
-D = zeros(6,4);