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:37:09 +02:00
parent b3aaf40487
commit a0eeeba787
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
selflocalization/selflocalize.py
View File

@ -254,8 +254,16 @@ try:
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 = particle.estimate_pose(particles) # The estimate of the robots current pose
calc_weight(est_pose, landmark_values) calc_weight(est_pose, landmark_values)
if est_pose.weight > 10**-23: est_weight = est_pose.weight * 10**23
print(est_weight)
if est_weight > 1:
break break
else:
arlo.go_diff(POWER, POWER, 0, 1)
sleep((10 * TURN_T)/1000)
arlo.stop()
for p in particles:
p.setTheta(p.theta + np.deg2rad(10))
if showGUI: if showGUI:
# Draw map # Draw map