From dc8f23bd5e4744179a0b76873082af8c5b468c49 Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Mon, 10 Oct 2022 14:27:09 +0200 Subject: [PATCH] :sparkles: --- selflocalization/particle.py | 2 +- selflocalization/selflocalize.py | 8 +++----- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/selflocalization/particle.py b/selflocalization/particle.py index 7534b92..c662181 100644 --- a/selflocalization/particle.py +++ b/selflocalization/particle.py @@ -67,7 +67,7 @@ def estimate_pose(particles_list): sin_certainty = np.average([abs(np.sin(theta) - i) for i in sin_values]) certainty = np.average([x_certainty, y_certainty, cos_certainty, sin_certainty]) - return Particle(x, y, theta), certainty + return Particle(x, y, theta) def move_particle(particle, delta_x, delta_y, delta_theta): diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py index 7ef198b..397dede 100644 --- a/selflocalization/selflocalize.py +++ b/selflocalization/selflocalize.py @@ -199,7 +199,7 @@ try: num_particles = 2000 particles = initialize_particles(num_particles) - est_pose, _ = particle.estimate_pose(particles) # The estimate of the robots current pose + est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose # Driving parameters velocity = 0.0 # cm/sec @@ -252,10 +252,8 @@ try: p.setWeight(1.0/num_particles) particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) - est_pose, certainty = particle.estimate_pose(particles) # The estimate of the robots current pose - print("Certainty: ", certainty) - if certainty < 1: - break + est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose + print(calc_weight(est_pose, landmark_values)) if showGUI: # Draw map