diff --git a/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py index edd4f8798e2bcadc2c9449f06d58e7c4c044dacc..6cd07d0cbe0ee518c20595d20a68f5ba584f0093 100644 --- a/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py +++ b/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)