pyquaternion icon indicating copy to clipboard operation
pyquaternion copied to clipboard

Integration not behaving as expected

Open cinabars opened this issue 5 years ago • 0 comments

Hi, I'm having unexpected behavior that could just be me misusing the library, or could be a problem with the library itself. Either way, I'd greatly appreciate if someone could help me understand what is going on, and sorry if this kind of question doesn't belong here.

I have an object with an initial orientation described by orientation (active rotation from global to body axis). I then get a body angular rate measurement (e.g. from a gyro) and want to apply this to get the new orientation. So I have this test case, in which I start with positive pitch and apply a roll on the body axis:

def test_integrate_quaternion_pitch_roll():
    # Arrange
    # Start with positive pitch
    orientation = Quaternion(axis=[0, 1, 0], degrees=80).unit
    # Roll on body axis
    angular_rates_body = np.deg2rad(np.array([30, 0, 0])).T
    delta_t = 1

    # Act
    angular_rates_global = orientation.inverse.rotate(angular_rates_body)
    orientation.integrate(angular_rates_global, delta_t)

    print(np.rad2deg(orientation.yaw_pitch_roll))

This gives me the expected yaw_pitch_roll of [ 0. 80. 30.] in degrees.

Next, I have this test case, where I start with a positive roll and apply a pitch on the body axis:

def test_integrate_quaternion_roll_pitch():
    # Arrange
    # Start with positive roll
    orientation = Quaternion(axis=[1, 0, 0], degrees=80).unit
    # Pitch on body axis
    angular_rates_body = np.deg2rad(np.array([0, 30, 0])).T
    delta_t = 1

    # Act
    angular_rates_global = orientation.inverse.rotate(angular_rates_body)
    orientation.integrate(angular_rates_global, delta_t)

    print(np.rad2deg(orientation.yaw_pitch_roll))

My expected result would be something with positive yaw, a small amount of positive pitch, and positive roll. Instead, I get [-29.62165188 4.98092532 81.3177961 ], i.e. the yaw appears to be negated.

What's going wrong?

cinabars avatar Dec 31 '19 23:12 cinabars