# Loop over all motor tick records. # This is the FastSLAM filter loop, with prediction and correction. f = open("fast_slam_correction.txt", "w") for i in xrange(len(logfile.motor_ticks)): # Prediction. control = map(lambda x: x * ticks_to_mm, logfile.motor_ticks[i]) fs.predict(control) # Correction. cylinders = get_cylinders_from_scan(logfile.scan_data[i], depth_jump, minimum_valid_distance, cylinder_offset) fs.correct(cylinders) # Output particles. print_particles(fs.particles, f) # Output state estimated from all particles. mean = get_mean(fs.particles) print >> f, "F %.0f %.0f %.3f" %\ (mean[0] + scanner_displacement * cos(mean[2]), mean[1] + scanner_displacement * sin(mean[2]), mean[2]) # Output error ellipse and standard deviation of heading. errors = get_error_ellipse_and_heading_variance(fs.particles, mean) print >> f, "E %.3f %.0f %.0f %.3f" % errors # Output landmarks of particle which is closest to the mean position. output_particle = min([ (np.linalg.norm(mean[0:2] - fs.particles[i].pose[0:2]), i)
control_motion_factor, control_turn_factor) # Read data. logfile = LegoLogfile() logfile.read("robot4_motors.txt") # Loop over all motor tick records. # This is the FastSLAM filter loop, prediction only. f = open("fast_slam_prediction.txt", "w") for i in range(len(logfile.motor_ticks)): # Prediction. control = map(lambda x: x * ticks_to_mm, logfile.motor_ticks[i]) fs.predict(control) # Output particles. print_particles(fs.particles, f) # Output state estimated from all particles. mean = get_mean(fs.particles) #print >> f, "F %.0f %.0f %.3f" %\ # (mean[0] + scanner_displacement * cos(mean[2]), # mean[1] + scanner_displacement * sin(mean[2]), # mean[2]) print ("F %.0f %.0f %.3f" %\ (mean[0] + scanner_displacement * cos(mean[2]), mean[1] + scanner_displacement * sin(mean[2]), mean[2]), end = "\n", file = f) # Output error ellipse and standard deviation of heading. errors = get_error_ellipse_and_heading_variance(fs.particles, mean)