# Correction. observations = get_observations(logfile.scan_data[i], depth_jump, minimum_valid_distance, cylinder_offset, kf, max_cylinder_distance) for obs in observations: measurement, cylinder_world, cylinder_scanner, cylinder_index = obs if cylinder_index == -1: cylinder_index = kf.add_landmark_to_state(cylinder_world) kf.correct(measurement, cylinder_index) # End of EKF SLAM - from here on, data is written. # Output the center of the scanner, not the center of the robot. print >> f, "F %f %f %f" % \ tuple(kf.state[0:3] + [scanner_displacement * cos(kf.state[2]), scanner_displacement * sin(kf.state[2]), 0.0]) # Write covariance matrix in angle stddev1 stddev2 stddev-heading form. e = ExtendedKalmanFilterSLAM.get_error_ellipse(kf.covariance) print >> f, "E %f %f %f %f" % (e + (sqrt(kf.covariance[2, 2]), )) # Write estimates of landmarks. write_cylinders(f, "W C", kf.get_landmarks()) # Write error ellipses of landmarks. write_error_ellipses(f, "W E", kf.get_landmark_error_ellipses()) # Write cylinders detected by the scanner. write_cylinders(f, "D C", [(obs[2][0], obs[2][1]) for obs in observations]) f.close()
# Read data. logfile = LegoLogfile() logfile.read("robot4_motors.txt") # Loop over all motor tick records and all measurements and generate # filtered positions and covariances. # This is the EKF SLAM loop. f = open("ekf_slam_add_landmarks.txt", "w") for i in xrange(len(logfile.motor_ticks)): # Prediction. control = array(logfile.motor_ticks[i]) * ticks_to_mm kf.predict(control) # End of EKF SLAM - from here on, data is written. # Output the center of the scanner, not the center of the robot. print >> f, "F %f %f %f" % \ tuple(kf.state[0:3] + [scanner_displacement * cos(kf.state[2]), scanner_displacement * sin(kf.state[2]), 0.0]) # Write covariance matrix in angle stddev1 stddev2 stddev-heading form e = ExtendedKalmanFilterSLAM.get_error_ellipse(kf.covariance) print >> f, "E %f %f %f %f" % (e + (sqrt(kf.covariance[2,2]),)) # Write estimates of landmarks. write_cylinders(f, "W C", kf.get_landmarks()) # Write error ellipses of landmarks. write_error_ellipses(f, "W E", kf.get_landmark_error_ellipses()) f.close()
# Read data. logfile = LegoLogfile() logfile.read("robot4_motors.txt") # Loop over all motor tick records and all measurements and generate # filtered positions and covariances. # This is the EKF SLAM loop. f = open("ekf_slam_add_landmarks.txt", "w") for i in range(len(logfile.motor_ticks)): # Prediction. control = array(logfile.motor_ticks[i]) * ticks_to_mm kf.predict(control) # End of EKF SLAM - from here on, data is written. # Output the center of the scanner, not the center of the robot. print("F %f %f %f" % \ tuple(kf.state[0:3] + [scanner_displacement * cos(kf.state[2]), scanner_displacement * sin(kf.state[2]), 0.0]), file=f) # Write covariance matrix in angle stddev1 stddev2 stddev-heading form e = ExtendedKalmanFilterSLAM.get_error_ellipse(kf.covariance) print("E %f %f %f %f" % (e + (sqrt(kf.covariance[2, 2]), )), file=f) # Write estimates of landmarks. write_cylinders(f, "W C", kf.get_landmarks()) # Write error ellipses of landmarks. write_error_ellipses(f, "W E", kf.get_landmark_error_ellipses()) f.close()
# Correction. observations = get_observations( logfile.scan_data[i], depth_jump, minimum_valid_distance, cylinder_offset, kf, max_cylinder_distance) for obs in observations: measurement, cylinder_world, cylinder_scanner, cylinder_index = obs if cylinder_index == -1: cylinder_index = kf.add_landmark_to_state(cylinder_world) kf.correct(measurement, cylinder_index) # End of EKF SLAM - from here on, data is written. # Output the center of the scanner, not the center of the robot. print >> f, "F %f %f %f" % \ tuple(kf.state[0:3] + [scanner_displacement * cos(kf.state[2]), scanner_displacement * sin(kf.state[2]), 0.0]) # Write covariance matrix in angle stddev1 stddev2 stddev-heading form. e = ExtendedKalmanFilterSLAM.get_error_ellipse(kf.covariance) print >> f, "E %f %f %f %f" % (e + (sqrt(kf.covariance[2,2]),)) # Write estimates of landmarks. write_cylinders(f, "W C", kf.get_landmarks()) # Write error ellipses of landmarks. write_error_ellipses(f, "W E", kf.get_landmark_error_ellipses()) # Write cylinders detected by the scanner. write_cylinders(f, "D C", [(obs[2][0], obs[2][1]) for obs in observations]) f.close()