fix method orientation_voiture

PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py

@@ -99,15 +99,20 @@ def distance_Chamfer(A_x, A_y, B_x, B_y):
 
 
 def orientation_voiture(x,y):
-    x1,y1 = x-centre_x, y-centre_y
+    x1, y1=x,y
+    #x1,y1 = x-centre_x, y-centre_y
+    #print(f"1: x: {x1}, y: {y1}")
+    
     if x<0:
         x2,y2 = x1+diametre/2, y1
+        #print(f"2: x: {x2}, y: {y2}")
         theta = atan(y2/x2)
         return theta-3.1415/2
     else:
         x2,y2 = x1-diametre/2, y1
+        #print(f"2: x: {x2}, y: {y2}")
         theta = atan(y2/x2)
-        return theta-3.1415/2
+        return theta+3.1415/2
     
     
 def motion_update(commande, position):
@@ -134,6 +139,7 @@ def motion_update(commande, position):
         
         new_x, new_y = rotation((x,y),(centre_rotation_x, centre_rotation_y), angle_rotation)
         new_theta = orientation_voiture(new_x, new_y)
+        #print(new_theta)
     
     new_x += rd.gauss(0, sigma_position)
     new_y += rd.gauss(0, sigma_position)