python3dquaternionsgyroscoperotational-matrices

quaternion rotation vs. rotation matrix have a slight difference


I have a 3D rotation over time represented as a momentary rotation around each of the axis (roll, pitch, yaw).

I'm trying to accumulate this rotation over time (about 50k measurements in total). I've tried doing it in 2 different ways. Using rotation matrices, and using quaternions calculation. The rotation matrices implementation seem to give correct results, but I know it is less recommended for accumulating many rotations.

The 2 results seem quite similar, but it accumulates a slight difference between the 2 results over time (about 1 degree every 250 measurements). I'm not sure where this difference comes from. Whether it is caused by floating point precision in calculating many matrices multiplications, or by using wrong parameters for the quaternion initialization.

This is the code I use:

# Last known rotation. As quaternion and as rotation matrix
last_rotation_matrix = ....
last_quaternion_rotation = .....

# time_diff_seconds is approximately 4/1000 
# the momentary rotation speed is around 0-1 radian per second. so [roll,pitch,yaw] are around 0-0.004)
roll = rotation_x_speed * time_diff_seconds
pitch = rotation_y_speed * time_diff_seconds
yaw = rotation_z_speed * time_diff_seconds
total_rotation = np.sqrt(roll**2 + pitch**2 + yaw**2)

# This function creates a rotation matrix based on the given parameters
diff_rotation_matrix = rotation_matrix(roll, pitch, yaw)
# THIS IS THE LINE THAT I SUSPECT:
diff_quaternion_rotation = Quaternion(axis=[rotation_x_speed, rotation_y_speed, rotation_z_speed], radians=total_rotation)

new_rotation_matrix = diff_quaternion_rotation.dot(last_rotation_matrix)
new_quaternion_rotation = diff_quaternion_rotation * last_rotation_matrix

The line that I suspect is the line initializing the diff_quaternion_rotation variable.


Solution

  • total_rotation = np.sqrt(roll**2 + pitch**2 + yaw**2)

    This is wrong - Euler angles cannot be added in this way. Neither is your axis calculation correct.

    Instead there is an explicit algorithm for converting Euler angles to quaternions:

    1

    (If your custom library doesn't have this function):

    cy, sy = cos(yaw * 0.5), sin(yaw * 0.5)
    cr, sr = cos(roll * 0.5), sin(roll * 0.5)
    cp, sp = cos(pitch * 0.5), sin(pitch * 0.5)
    
    diff_quaternion_rotation = Quaternion(w = cy * cr * cp + sy * sr * sp,
                                          x = cy * sr * cp - sy * cr * sp,
                                          y = cy * cr * sp + sy * sr * cp,
                                          z = sy * cr * cp - cy * sr * sp)