# 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()
