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)
for i in xrange(len(fs.particles))
])[1]
# Write estimates of landmarks.
write_cylinders(f, "W C",
fs.particles[output_particle].landmark_positions)
# Write covariance matrices.
write_error_ellipses(
f, "W E", fs.particles[output_particle].landmark_covariances)
f.close()
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)
for i in xrange(len(fs.particles)) ])[1]
# Write estimates of landmarks.
write_cylinders(f, "W C",
fs.particles[output_particle].landmark_positions)
# Write covariance matrices.
write_error_ellipses(f, "W E",
fs.particles[output_particle].landmark_covariances)
f.close()
