diff --git a/models/spiri/spiri.sdf b/models/spiri/spiri.sdf
index 769f9ce..f94ebb4 100644
--- a/models/spiri/spiri.sdf
+++ b/models/spiri/spiri.sdf
@@ -2,7 +2,7 @@
 <sdf version='1.6'>
   <model name='spiri'>
 	    <link name='base'>
-	      	<pose frame=''>0 0 0 0 -0 3.141</pose>
+	      	<pose frame=''>0 0 0 0 -0 0</pose>
 		<gravity>1</gravity>
 	      	<inertial>
 		        <pose frame=''>0 0.00268 -0.000742 0 -0 0</pose>
@@ -408,6 +408,118 @@
 	       <child>gps0_link</child>
 	     </joint>
 	     </model>
+  <!--model name="sonar">
+    <link name="sonar_link">
+      <pose>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.000000000000001</mass>
+        <inertia>
+          <ixx>2.08333333e-7</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>2.08333333e-7</iyy>
+          <iyz>0</iyz>
+          <izz>4.16666667e-8</izz>
+        </inertia>
+      </inertial>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.005 0.015 0.005</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Black</name>
+          </script>
+        </material>
+      </visual>
+      <sensor name="sonar" type="sonar">
+        <pose>0 0 0 0 0 0</pose>
+        <sonar>
+          <min>0.02</min>
+          <max>10.0</max>
+          <radius>1.33974388395</radius>
+        </sonar>
+        <plugin name="SonarPlugin" filename="libgazebo_sonar_plugin.so">
+          <robotNamespace></robotNamespace>
+    	  	<topic>/height_cesar</topic>
+        </plugin>
+        <always_on>1</always_on>
+        <update_rate>20</update_rate>
+        <visualize>true</visualize>
+      </sensor>
+    </link>
+  </model>
+	    <joint name='sonar_joint' type='fixed'>
+	      <parent>base</parent>
+	      <child>sonar_link</child>
+	    </joint-->
+<!--sonar-->
+  <link name="sonar_link">
+    <pose frame=''>0 0 0 0 1.57 0</pose>
+    <inertial> <!-- causes issues when omitted -->
+      <mass>0.05</mass>
+      <inertia>
+        <ixx>2.08333333333e-07</ixx>
+        <iyy>5.20833333333e-07</iyy>
+        <izz>5.20833333333e-07</izz>
+      </inertia>
+    </inertial>
+    <visual name="sonar_link_visual">
+      <pose frame=''>0 0 0 0 0 0</pose>
+      <geometry>
+        <cylinder>
+          <length>0.008</length>
+          <radius>0.004</radius>
+        </cylinder>
+      </geometry>
+      <material>
+        <script>
+          <name>Gazebo/Blue</name>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+        </script>
+      </material>
+    </visual>
+    <sensor type="ray" name="spiri_sonar">
+      <update_rate>10</update_rate>
+      <visualize>true</visualize>
+      <ray>
+        <scan>
+          <horizontal>
+            <samples>1</samples>
+            <resolution>1</resolution>
+            <min_angle>-0</min_angle>
+            <max_angle>0</max_angle>
+          </horizontal>
+        </scan>
+        <range>
+          <min>0.06</min> <!-- do not lower past 0.06 -->
+          <max>35</max>   <!-- do not increase past 35 -->
+          <resolution>0.01</resolution>
+        </range>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.01</stddev>
+        </noise>
+      </ray>
+      <plugin name="spiri_sonar" filename="libgazebo_ros_range.so">
+        <gaussianNoise>0.005</gaussianNoise>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>10</updateRate>
+        <topicName>/mavros/distance_sensor</topicName>
+        <frameName>sonar_link</frameName>
+        <fov>0</fov>
+        <radiation>ultrasound</radiation>
+      </plugin>
+    </sensor>
+  </link>
+  <joint name="sonar_joint" type="fixed">
+    <parent>base</parent>
+    <child>sonar_link</child>
+  </joint>
 	    <plugin name='rosbag' filename='libgazebo_multirotor_base_plugin.so'>
 	      <robotNamespace/>
 	      <linkName>base</linkName>
diff --git a/src/klausen_control.py b/src/klausen_control.py
index 376c18b..13a88a3 100755
--- a/src/klausen_control.py
+++ b/src/klausen_control.py
@@ -101,8 +101,10 @@ class Main:
 		
 		# PD Thrust Controller terms
 		# gains for thrust PD Controller
-		self.Kp 	= 2.7 
-		self.Kd 	= 3 #2
+		#self.Kp 	= 2.7 
+		#self.Kd 	= 3 
+		self.Kp 	= 2.7
+		self.Kd 	= 3 
 		self.max_a	= 14.2
 		self.max_t  = self.drone_m*self.max_a
 		self.R 		= np.empty([3,3]) # rotation matrix
@@ -252,15 +254,22 @@ class Main:
 		# Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
 		self.waypoints_srv_cb()
 		self.error = np.array([[self.dr_pos.position.x - self.xd.x],
-							[self.dr_pos.position.y - self.xd.y],
-							[self.dr_pos.position.z - self.xd.z - self.thrust_offset]]) 
+								[self.dr_pos.position.y - self.xd.y],
+								[self.dr_pos.position.z - self.xd.z - self.thrust_offset]])
 
-		self.R = self.quaternion_rotation_matrix()	# determine Rotation Matrix 
+		self.error_vel = self.dr_vel - self.path_vel
+
+		self.R = self.quaternion_rotation_matrix()	# determine Rotation Matrix
+		self.R_e3 =  np.array([[self.R.T[2][0]],[self.R.T[2][1]],[self.R.T[2][2]]])
 
 
-		#thrust = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel.T,self.R.T[2])/self.max_t
-		thrust_vector = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel.T,self.R.T[2])/self.max_t
-		thrust = thrust_vector[2]
+		#thrust = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel,self.R.T[2])/self.max_t
+		#thrust_vector = np.dot(9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel,self.R_e3)/self.max_t
+		#thrust_vector = (9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel -9.81*self.drone_m*self.path_acc)/self.max_t
+		thrust_vector = (9.81*self.drone_m - self.Kp*self.error[2] - self.Kd*self.dr_vel[2])/self.max_t
+		#thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2]
+		thrust = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[1]))
+		#thrust = thrust_vector[2]
 
 		# Value needs to be between 0 - 1.0
 		self.att_targ.thrust = max(0.0,min(thrust,1.0))