particles = Particle.create_random(PARTICLE_COUNT, world)
robbie = Robot(world)
robbie.x = 246
robbie.y = 110
step_count = 0
for step in pixel_array:
# print("Step count:", step_count)
step_count += 1
# r_d = robbie.read_sensor(world)
r_d = np.array(robbie.read_all_sensors(world))
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)
