# Loop over all motor tick records.
# This is the FastSLAM filter loop, with prediction and correction.
f = open("fast_slam_correction.txt", "w")
for i in xrange(len(logfile.motor_ticks)):
# Prediction.
control = map(lambda x: x * ticks_to_mm, logfile.motor_ticks[i])
fs.predict(control)
# Correction.
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)
control_motion_factor, control_turn_factor)
# Read data.
logfile = LegoLogfile()
logfile.read("robot4_motors.txt")
# 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)
