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.
