def perform_one_iteration(self): """Perform one integration iteration: read, integrate to compute update, and print if necessary.""" if self.verbose > 0: print("\n### NEW CYCLE " + str(self.cycle) + " ###\n") self.predict_state() self.read_and_update_measurement() # normalise orientation quaternion: this can be necessary if some noise # due to for exemple nearly singular matrix that get inverted self.o = qt.normalise_quaternion(self.o) if self.verbose > 0: print("PRINT STATE AT END INTEGRATION CYCLE ###") self.print_state(self.o) self.cycle += 1
def __init__(self, ReadFromArduino_instance, dt, offset_GYRO, verbose=0): self.dt = dt self.verbose = verbose self.ReadFromArduino_instance = ReadFromArduino_instance self.list_measurements = [] self.offset_GYRO = offset_GYRO # initial state: assume resting, Z downwards, still -------------------- # no rotation to Earth referential self.o = qt.normalise_quaternion( qt.Quaternion(1, 0.0001, 0.0001, 0.0001)) # reference vectors; can be used to print assumed orientation ---------- self.X_IMU_ref_IMU = qt.Vector(1, 0, 0) self.Y_IMU_ref_IMU = qt.Vector(0, 1, 0) self.Z_IMU_ref_IMU = qt.Vector(0, 0, 1) self.read_and_update_measurement() self.cycle = 0