scan = spilted_scandata[2:]
scan_data = []
#scan_data.append('S')
for i in range(len(scan)):
scan_data.append(int(scan[i]))
#.................................
write_scanData(f, "S", scan_data)
#...........................perdict
control = map(lambda x: x * ticks_to_mm,
fs.get_tiks(encode_data))
fs.predict(control)
#...........................
#...........................correct
cylinders = get_cylinders_from_scan(scan_data, depth_jump,
minimum_valid_distance,
cylinder_offset)
fs.correct(cylinders)
# ..................................
#................................... data representation
#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.
# Read data.
logfile = LegoLogfile()
logfile.read("robot4_motors.txt")
logfile.read("robot4_scan.txt")
# 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)
# Read data.
logfile = LegoLogfile()
logfile.read("robot4_motors.txt")
logfile.read("robot4_scan.txt")
# 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
