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