예제 #1
0
    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
예제 #2
0
    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