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 dump_measurements(plan_list, robot_pos, iter_count, Z):
f = open('sonar_meas'+str(iter_count).zfill(3)+'.dat', 'w')
print "Sonars:", Z
write_meas(robot_pos, Z, f)
f.close()
mes = lineutils.measurements(plan_list, robot_pos)
f = open('ideal_meas'+str(iter_count).zfill(3)+'.dat', 'w')
write_meas(robot_pos, mes, f)
f.close()
def dump_measurements(plan_list, robot_pos, iter_count, Z):
f = open('sonar_meas'+str(iter_count).zfill(3)+'.dat', 'w')
print ("Sonars:", Z)
write_meas(robot_pos, Z, f)
f.close()
mes = lineutils.measurements(plan_list, robot_pos)
f = open('ideal_meas'+str(iter_count).zfill(3)+'.dat', 'w')
write_meas(robot_pos, mes, f)
f.close()
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
