def measurement_prob(plan_list, particle, measurements):
# exclude particles outside the room
if not lineutils.point_inside_polygon(plan_list, particle):
return 0.0
# calculate the correct measurement
predicted_measurements = lineutils.measurements(room_plan, particle)
# compute errors
prob = 1.0
count = 0
for i in xrange(0, len(measurements)):
if measurements[i] != 0:
error_mes = abs(measurements[i] - predicted_measurements[i])
# update Gaussian
#error *= (exp(- (error_mes ** 2) / (bearing_noise ** 2) / 2.0) / sqrt(2.0 * pi * (bearing_noise ** 2)))
#8/28
prob += (exp(-(error_mes**2) / (bearing_noise**2) / 2.0) /
sqrt(2.0 * pi * (bearing_noise**2)))
count += 1
prob /= count
prob = prob**4
return prob
def particle_filter(motions, path, goal, N=1500):
# Make particles
world_x_size = abs(world_x_range[1] - world_x_range[0])
world_y_size = abs(world_y_range[1] - world_y_range[0])
p = []
while len(p) < N:
# particle is a vector (x,y,bearing)
particle = (random.random() * world_x_size + world_x_range[0],
random.random() * world_y_size + world_y_range[0],
random.random() * 2.0 * pi)
# add only particles inside our room
if lineutils.point_inside_polygon(room_plan, particle):
p.append(particle)
# Update particles
iter_count = 0
#2014/09/09
complete = 1
#2014/09/09{
#complete : 0, not complete : 1
while(complete == 1):
information = prediction_and_measurement(motions, path, goal, N, p, iter_count, complete)
motions = information[0]
path = information[1]
p = information[2]
iter_count = information[3]
complete = information[4]
#2014/09/09}
return get_position(p)
def measurement_prob(plan_list, particle, measurements):
# exclude particles outside the room
if not lineutils.point_inside_polygon(plan_list, particle):
return 0.0
# calculate the correct measurement
predicted_measurements = lineutils.measurements(room_plan, particle)
# compute errors
error = 1.0
for i in xrange(1, len(measurements)):
error_mes = abs(measurements[i] - predicted_measurements[i])
# update Gaussian
error *= (exp(- (error_mes ** 2) / (bearing_noise ** 2) / 2.0) / sqrt(2.0 * pi * (bearing_noise ** 2)))
return error
def particle_filter(chassis, sensor_receiver, motions, N=10000):
# Make particles
world_x_size = abs(world_x_range[1] - world_x_range[0])
world_y_size = abs(world_y_range[1] - world_y_range[0])
p = []
while len(p) < N:
# particle is a vector (x,y,bearing)
particle = (random.random() * world_x_size + world_x_range[0],
random.random() * world_y_size + world_y_range[0],
random.random() * 2.0 * pi)
# add only particles inside our room
if lineutils.point_inside_polygon(room_plan, particle):
p.append(particle)
# Update particles
iter_count = 0;
for motion in motions:
# Motion update (prediction)
p = map(lambda particle: move(particle, motion), p)
# Now move the real vehicle
#
# Calculating path need to be made by each wheel for the turn.
# We assuming that Sl = -Sr, i.e. in-place turn by rotating wheels
# in the opposite directions
(delta_theta, s, _) = motion
sr = (delta_theta) * robot_width
sl = -sr
# Build commands for the vehicle to rotate for delta_theta and
# then drive request distance s
make_motion_step(chassis, sl, sr) # Rotate on place
make_motion_step(chassis, s, s) # Drive forward
# Measurement update
Z = sensor_receiver.sonars
w = map(lambda particle: measurement_prob(room_plan, particle, Z), p)
# Resampling
p2 = []
index = int(random.random() * N)
beta = 0.0
mw = max(w)
for i in range(N):
beta += random.random() * 2.0 * mw
while beta > w[index]:
beta -= w[index]
index = (index + 1) % N
p2.append(p[index])
p = p2
# Dump current particle set to file for plotting
(_,_,robot_ideal_pos) = motion
dump_measurements(room_plan, robot_ideal_pos, iter_count, Z)
# Dump current particle set to file for plotting
f = open('iteration'+str(iter_count).zfill(3)+'.dat', 'w')
map(lambda (x,y,_): f.write(str(x)+'\t'+str(y)+'\n'), p)
f.close()
iter_count += 1
return get_position(p)
