NikolajDanger/Roboteksperimentarium
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-10-10 14:38:55 +02:00
parent a0eeeba787
commit 86b621fa9d
2 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
selflocalization/particle.py
View File

@ -67,7 +67,7 @@ def estimate_pose(particles_list):
sin_certainty = np.average([abs(np.sin(theta) - i) for i in sin_values]) 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]) certainty = np.average([x_certainty, y_certainty, cos_certainty, sin_certainty])
return Particle(x, y, theta) return Particle(x, y, theta), certainty
def move_particle(particle, delta_x, delta_y, delta_theta): def move_particle(particle, delta_x, delta_y, delta_theta):

6
selflocalization/selflocalize.py
View File

@ -199,7 +199,7 @@ try:
num_particles = 2000 num_particles = 2000
particles = initialize_particles(num_particles) 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 # Driving parameters
velocity = 0.0 # cm/sec velocity = 0.0 # cm/sec
@ -252,11 +252,11 @@ try:
p.setWeight(1.0/num_particles) p.setWeight(1.0/num_particles)
particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose est_pose, certainty = particle.estimate_pose(particles) # The estimate of the robots current pose
calc_weight(est_pose, landmark_values) calc_weight(est_pose, landmark_values)
est_weight = est_pose.weight * 10**23 est_weight = est_pose.weight * 10**23
print(est_weight) print(est_weight)
if est_weight > 1: if est_weight > 1 and certainty > 1:
break break
else: else:
arlo.go_diff(POWER, POWER, 0, 1) arlo.go_diff(POWER, POWER, 0, 1)