diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index b40ad649..41b9d525 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -216,7 +216,7 @@ def observation(xTrue, xd, u, RFID): dx = RFID[i, 0] - xTrue[0, 0] dy = RFID[i, 1] - xTrue[1, 0] - d = math.sqrt(dx**2 + dy**2) + d = math.hypot(dx, dy) angle = pi_2_pi(math.atan2(dy, dx)) - xTrue[2, 0] phi = pi_2_pi(math.atan2(dy, dx)) if d <= MAX_RANGE: