Fixing odometrie problem

driving.py

@@ -11,7 +11,6 @@ import numpy as np
 S = 0 # Value for a straight part of the track
 R = 1 # Value for a right turn
 L = 2 # Value for a left turn
-
 #%% Classes
 """
 # This class contains some functions to analyze data from the LEGO robot EV3.
@@ -86,8 +85,8 @@ class Driving_Analyser:
 
         else:
             theta = self.theta_old
-            x = self.x_old + (v * np.cos(self.theta_old) * time_delta)
-            y = self.y_old + (v * np.sin(self.theta_old) * time_delta)
+            x = self.x_old + (v * np.cos(theta) * time_delta)
+            y = self.y_old + (v * np.sin(theta) * time_delta)
 
 
         self.theta_old = theta
@@ -105,7 +104,7 @@ class Driving_Analyser:
         if w != 0:
             theta_delta = (w * time_delta)
             x_delta = (v * (np.sin(theta_delta) / w))
-            y_delta = (v * (-(np.cos(theta_delta) + cos(0)) / w))
+            y_delta = (v * (-(np.cos(theta_delta) - np.cos(0)) / w))
 
         else:
             theta_delta = 0