world.draw() # initial distribution assigns each particle an equal probability particles = Particle.create_random(PARTICLE_COUNT, world) robbie = Robot(world) raw_input() while True: # Read robbie's sensor r_d = 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 = 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 = []
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 ----------
# resample state_prime = [] eta = 0 for _ in range(0, N): while True: # j ~ {w} with replacement sj = dist.pick() # x' ~ P( x' | U, sj ) x_prime = Particle(add_some_noise(sj.x + robot.dx, sj.y + robot.dy)) # loop to discard particles in impossible places (occupied cells) because # our probability distribution distribution doesn't know about occupancy if world.is_free(*x_prime.xy): break # w' = P( z | x' ); 1 is close to the robot's measurement, 0 is farther away particle_z = x_prime.read_sensor(world) error = z - particle_z w_prime = math.e ** -(error ** 2 / (2 * StdDev ** 2)) x_prime.w = w_prime z_estimate += particle_z * w_prime # accumulate normalizer eta += w_prime # add to new state state_prime.append(x_prime)
# 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 # number in the picking phase below particles = [p for p in particles if m.is_free(*p.xy)] delta_p = PARTICLE_COUNT - len(particles) # Pick N particles according to weight, duplicate them and add # some noise to their position new_particles = [] for cnt in range(0, N): p = weightedPick(particles) new_particles.append(Particle(p.x, p.y)) # Add random new ones so the overall count fits, we might have # eliminated some while moving new_particles += Particle.create_random(delta_p, m) # Add some random noise for p in new_particles:
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 = 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 ): #not sure what is world.is_free() actually means in real word and in the program p_d = p.read_sensor(world) p.w = w_gauss( r_d, p_d) #compute Euclidean distance between particle and robot 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)
#print(p.x,p.y) weight = 0 for bleSignal in beaconTable: blePos = [bleSignal['x'], bleSignal['y']] rssi = bleSignal['rssi'] Loss = (abs(rssi) + 40) / 20 likelyhood = 1000 * (10**(-Loss)) particlePixelPos = mg.mazeGridToPixel(p.x, p.y) #print("hello ", blePos) dis = euclideanDistance(blePos, particlePixelPos) #print(dis) weight += huber_gauss(dis, 80) * likelyhood p.w = weight #print(p.w) for p in particles: if not world.is_free(*p.xy): p.w = 0 # ---------- Try to find current best estimate for display ---------- m_x, m_y, m_confident = compute_mean_point(particles) #print(m_x,m_y) # ---------- 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)