# Setup filter.
pf = ParticleFilter(initial_particles, robot_width, scanner_displacement,
control_motion_factor, control_turn_factor,
measurement_distance_stddev, measurement_angle_stddev)
# Read data.
logfile = LegoLogfile()
logfile.read("robot4_motors.txt")
logfile.read("robot4_scan.txt")
logfile.read("robot_arena_landmarks.txt")
reference_cylinders = [l[1:3] for l in logfile.landmarks]
# Loop over all motor tick records.
# This is the particle filter loop, with prediction and correction.
f = open("particle_filter_corrected.txt", "w")
for i in range(len(logfile.motor_ticks)):
# Prediction.
control = map(lambda x: x * ticks_to_mm, logfile.motor_ticks[i])
pf.predict(control)
# Correction.
cylinders = get_cylinders_from_scan(logfile.scan_data[i], depth_jump,
minimum_valid_distance,
cylinder_offset)
pf.correct(cylinders, reference_cylinders)
# Output particles.
pf.print_particles(f)
f.close()
pf = ParticleFilter(initial_particles,
robot_width, scanner_displacement,
control_motion_factor, control_turn_factor,
measurement_distance_stddev,
measurement_angle_stddev)
# Read data.
logfile = LegoLogfile()
logfile.read("robot4_motors.txt")
logfile.read("robot4_scan.txt")
logfile.read("robot_arena_landmarks.txt")
reference_cylinders = [l[1:3] for l in logfile.landmarks]
# Loop over all motor tick records.
# This is the particle filter loop, with prediction and correction.
f = open("particle_filter_corrected.txt", "w")
for i in xrange(len(logfile.motor_ticks)):
# Prediction.
control = map(lambda x: x * ticks_to_mm, logfile.motor_ticks[i])
pf.predict(control)
# Correction.
cylinders = get_cylinders_from_scan(logfile.scan_data[i], depth_jump,
minimum_valid_distance, cylinder_offset)
pf.correct(cylinders, reference_cylinders)
# Output particles.
pf.print_particles(f)
f.close()
