for p in particles:
if world.is_free(*p.xy):
# p_d = np.array(p.read_all_sensors(world))
# error = math.sqrt(sum((r_d - p_d)**2))
# p.w *= w_gauss(0, error)
pass
else:
p.w = 0
# ---------- Try to find current best estimate for display ----------
m_x, m_y, m_confident = compute_mean_point(particles)
# ---------- Show current state ----------
# world.show_robot(robbie)
world.show_particles(particles)
world.show_mean(m_x, m_y, m_confident)
# world.show_robot(robbie)
# ---------- Shuffle particles ----------
new_particles = []
# Normalise weights
nu = sum(p.w for p in particles)
if nu:
for p in particles:
p.w = p.w / nu
# create a weighted distribution, for fast picking
dist = WeightedDistribution(particles)
