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)
