# 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) #print >> f, "E %.3f %.0f %.0f %.3f" % errors print ("E %.3f %.0f %.0f %.3f" % errors, end = "\n", file = f) f.close()
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) for i in xrange(len(fs.particles)) ])[1] # Write estimates of landmarks. write_cylinders(f, "W C", fs.particles[output_particle].landmark_positions) # Write covariance matrices. write_error_ellipses(f, "W E", fs.particles[output_particle].landmark_covariances) f.close()