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