Fixing accel / speed calc

DocGarbanzo · Sep 28, 2024 · b241923 · b241923
1 parent a1ac68d
commit b241923
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/donkeycar/parts/imu.py b/donkeycar/parts/imu.py
@@ -177,7 +177,9 @@ def poll(self):
         if not self.ahrs.flags.initialising:
             self.euler = self.ahrs.quaternion.to_euler()
             self.matrix = self.ahrs.quaternion.to_matrix()
-            self.lin_accel = np.dot(self.matrix, accel_phys) - self.accel_zero
+            self.lin_accel = np.dot(self.matrix, accel_phys)
+            # remove gravity from world coordinate z-axis
+            self.lin_accel[3] -= self.accel_zero[3]
             delta_v = self.lin_accel * dt
             self.speed += delta_v
             self.pos += self.speed * dt