pyquaternion
pyquaternion copied to clipboard
Integration not behaving as expected
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?