# if trafo:
# # print kf.state[1:4]
# kf.correct_pose(trafo)
# 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" % \
tuple(states[i] + [scanner_displacement * cos(states[i][2]),
scanner_displacement * sin(states[i][2]),
0.0])
# # For the purpose of putting out deviation.
# p0 = tuple(states[i] + [scanner_displacement * cos(states[i][2]),
# scanner_displacement * sin(states[i][2]),
# 0.0])
# deviation = distance(p0, logfile.reference_positions[i])
# if deviation > max_deviation:
# max_deviation = deviation
# Convert covariance matrix to angle stddev1 stddev2 stddev-heading form
e = ExtendedKalmanFilter.get_error_ellipse(covariances[i])
print >> f, "E %f %f %f %f" % (e + (sqrt(covariances[i][2, 2]), ))
# Also, write matched cylinders.
write_cylinders(f, "W C", matched_ref_cylinders[i])
f.close()
print max_deviation # For the purpose of putting out deviation.
# 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" % \
tuple(states[i] + [scanner_displacement * cos(states[i][2]),
scanner_displacement * sin(states[i][2]),
0.0])
# Convert covariance matrix to angle stddev1 stddev2 stddev-heading form
e = ExtendedKalmanFilter.get_error_ellipse(covariances[i])
print >> f, "E %f %f %f %f" % (e + (sqrt(covariances[i][2,2]),))
# Also, write matched cylinders.
write_cylinders(f, "W C", matched_ref_cylinders[i])
f.close()