diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py index 6092ce0..a55b9ae 100644 --- a/selflocalization/selflocalize.py +++ b/selflocalization/selflocalize.py @@ -254,10 +254,10 @@ try: particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) est_pose, certainty = particle.estimate_pose(particles) # The estimate of the robots current pose calc_weight(est_pose, landmark_values) - est_weight = est_pose.weight * 10**23 + est_weight = est_pose.weight * 10**10 print("est_weight:", est_weight) print("certainty:", certainty) - if est_weight > 1 and certainty > 1: + if est_weight > 1: break else: arlo.go_diff(POWER, POWER, 0, 1)