def main(): world = Maze(maze_data, HALF_WIDTH, HALF_HEIGHT) world.draw() for shark_count in range(151)[10::10]: # Initialize Items sharks = Shark.create_random(shark_count, world, 0) robert = Robot(world, 0,0) robots = [robert] no_particles = [] # Write to File my_file = open("%sSharksDistFromLine.txt" %(shark_count), "w") # Header describing model my_file.write("Line Start: %s, Line End: %s" %(LINE_START, LINE_END)) my_file.write("\n") my_file.write("NumSharks: %s, K_att: %s, K_rep: %s, Sigma_Rand: %s, Speed/ts: %s" %(shark_count, K_ATT, K_REP, SIGMA_RAND, Shark.speed)) my_file.write("\n") att_line = LineString([LINE_START, LINE_END]) # Let sharks move towards attraction line first for time_step in range(300): move(world, robots, sharks, att_line, no_particles, SIGMA_RAND, K_ATT, K_REP) # while True: for _ in range(3): for time_step in range(TIME_STEPS): # # ---------- Show current state ---------- if SHOW_VISUALIZATION: world.show_sharks(sharks) world.show_robot(robert) world.show_att_line(LINE_START, LINE_END) move(world, robots, sharks, att_line, no_particles, SIGMA_RAND, K_ATT, K_REP) for shark in sharks: # my_file.write("%s, %s," % (shark.x, shark.y)) my_file.write("%s, " %(distance_from_line(shark, att_line))) my_file.write("\n") print time_step my_file.close()
# 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 = 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 = [] # 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) for _ in particles: p = dist.pick()
self.move_by(dx, dy) return True return False # ------------------------------------------------------------------------ world = Maze(maze_data) world.draw() # Initialize Sharks sharks = Shark.create_random(SHARK_COUNT, world) print "length of shark", len(sharks) print sharks robert = Robot(world) while True: # ---------- Show current state ---------- world.show_sharks(sharks) world.show_robot(robert) # # ---------- Move things ---------- # Move sharks with shark's speed for s in sharks: s.advance(s.speed) time.sleep(0.1)
# 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 = [] # 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) for _ in particles: p = dist.pick()
world = Maze(maze_data) world.draw() # initial distribution assigns each particle an equal probability robot = Robot(world) state = Particle.create_random(N, world) StdDev = 1.4 estimate = False while True: # draw the state world.clear() world.show_particles(state) world.show_robot(robot) if estimate: world.show_estimate(estimate) world.update() # move randomly robot.move(world) # take measurement z = robot.read_sensor(world) # This is the weighted average of the particle estimates, and is used to determine if the model # converged to the correct location. This extension was not presented in class. z_estimate = 0 # create a weighted distribution, for fast picking
while True: # Read robbie's sensor r_d = robbie.read_sensor(m) # Update particle weight according to how good every particle matches # robbie's sensor reading for p in particles: p_d = p.read_sensor(m) # This is just a gaussian I pulled out of my hat, near to # robbie's measurement => 1, further away => 0 g = math.e ** -((r_d - p_d)**2 * 19) p.w = g # Time for some action! m.show_particles(particles) m.show_robot(robbie) # I'm doing the movement first, it makes life easier robbie.move(m) # Move particles according to my belief of movement (this may # be different than the real movement, but it's all I got) for p in particles: p.x += robbie.dx p.y += robbie.dy # ---------- Shuffle particles ---------- # Remove all particles that cannot be right (out of arena, inside # obstacle). Remember how many were removed so we can add that
dev = max(abs(d - r_d), 0.00001) w = 1 / dev particle.w = w #particle.w = w_gauss(d, r_d) weight_sum += particle.w for particle in particles: particle.w /= weight_sum # ---------- Show current state ---------- world.show_particles(particles) # world.show_mean(m_x, m_y, m_confident) world.show_robot(nao) # ---------- Shuffle particles ---------- new_particles = sus(particles) for p in new_particles: x, y = p.x, p.y p.x, p.y = add_some_noise(p.x, p.y) particles = new_particles # ---------- Move things ---------- old_heading = nao.h nao.move(world) d_h = nao.h - old_heading