logfile = LegoLogfile() logfile.read("robot4_motors.txt") logfile.read("robot4_scan.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_correction.txt", "w") for i in xrange(len(logfile.motor_ticks)): # Prediction. control = array(logfile.motor_ticks[i]) * ticks_to_mm kf.predict(control) # 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.
logfile = LegoLogfile() logfile.read("robot4_motors.txt") logfile.read("robot4_scan.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_correction.txt", "w") for i in xrange(len(logfile.motor_ticks)): # Prediction. control = array(logfile.motor_ticks[i]) * ticks_to_mm kf.predict(control) # 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.