scan = spilted_scandata[2:] scan_data = [] #scan_data.append('S') for i in range(len(scan)): scan_data.append(int(scan[i])) #................................. write_scanData(f, "S", scan_data) #...........................perdict control = map(lambda x: x * ticks_to_mm, fs.get_tiks(encode_data)) fs.predict(control) #........................... #...........................correct cylinders = get_cylinders_from_scan(scan_data, depth_jump, minimum_valid_distance, cylinder_offset) fs.correct(cylinders) # .................................. #................................... data representation #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.
# Read data. logfile = LegoLogfile() logfile.read("robot4_motors.txt") logfile.read("robot4_scan.txt") # 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)
# Read data. logfile = LegoLogfile() logfile.read("robot4_motors.txt") logfile.read("robot4_scan.txt") # 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