# Loop over all motor tick records and all measurements and generate
# filtered positions and covariances.
# This is the Kalman filter loop, with prediction and correction.
states = []
covariances = []
matched_ref_cylinders = []
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.state, scanner_displacement,
reference_cylinders, max_cylinder_distance)
for j in xrange(len(observations)):
kf.correct(*observations[j])
# Log state, covariance, and matched cylinders for later output.
states.append(kf.state)
covariances.append(kf.covariance)
matched_ref_cylinders.append([m[1] for m in observations])
# Write all states, all state covariances, and matched cylinders to file.
f = open("kalman_prediction_and_correction.txt", "w")
for i in xrange(len(states)):
# Output the center of the scanner, not the center of the robot.
print >> f, "F %f %f %f" % \
# Loop over all motor tick records and all measurements and generate
# filtered positions and covariances.
# This is the Kalman filter loop, with prediction and correction.
states = []
covariances = []
matched_ref_cylinders = [] # Magenta points.
for i in xrange(len(logfile.motor_ticks)): # For all steps.
# Prediction.
control = array(logfile.motor_ticks[i]) * ticks_to_mm
kf.predict(control) # Modify the instance kf to expected state.
# Correction.
observations = get_observations(
logfile.scan_data[i], depth_jump, minimum_valid_distance,
cylinder_offset, kf.state, scanner_displacement,
reference_cylinders, max_cylinder_distance
) # Detected(also the measurement) and real landmark pairs.
for j in xrange(len(observations)):
kf.correct(
*observations[j]) # Separate elements into several arguments.
# measurement = observation[j][0], landmark = observation[j][1]
# Log state, covariance, and matched cylinders for later output.
states.append(kf.state)
covariances.append(kf.covariance)
matched_ref_cylinders.append([m[1] for m in observations])
# det_car = []
# real_car = []
# trafo = []
# Loop over all motor tick records and all measurements and generate
# filtered positions and covariances.
# This is the Kalman filter loop, with prediction and correction.
states = []
covariances = []
matched_ref_cylinders = []
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.state, scanner_displacement,
reference_cylinders, max_cylinder_distance)
for j in xrange(len(observations)):
kf.correct(*observations[j])
# Log state, covariance, and matched cylinders for later output.
states.append(kf.state)
covariances.append(kf.covariance)
matched_ref_cylinders.append([m[1] for m in observations])
# Write all states, all state covariances, and matched cylinders to file.
f = open("kalman_prediction_and_correction.txt", "w")
for i in xrange(len(states)):
# Output the center of the scanner, not the center of the robot.
print >> f, "F %f %f %f" % \
