# 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()