# 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 = []