world = Maze(maze_data) world.draw() # initial distribution assigns each particle an equal probability particles = Particle.create_random(PARTICLE_COUNT, world) robbie = Robot(world) while True: # Read robbie's sensor r_d, r_d_x, r_d_y = robbie.read_sensor(world) # Update particle weight according to how good every particle matches # robbie's sensor reading for p in particles: if world.is_free(*p.xy): p_d = world.distance(r_d_x, r_d_y, p.x, p.y) #p_d = p.read_sensor(world) p.w = w_gauss(r_d, p_d) 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_particles(particles) world.show_mean(m_x, m_y, m_confident) world.show_robot(robbie) # ---------- Shuffle particles ---------- new_particles = []