Quaternion gradient does not seem to provide a reliable measure of angular velocity

[danielpmorton]

I was noticing some odd behavior from my quaternion gradients and decided to test the function against some baselines. It seems like there are some areas where pytransform's function lines up with the ground truth, but it quickly diverges significantly from the ground truth. Other methods are able to calculate the angular velocity with good tracking.

Here is a plot comparing angular velocities using pytransform3d, another quaternion method, and a ground-truth baseline. The other method lines up well with the baseline whereas pytransform does not

And here is the code to produce this (See the comments for cited info):

"""Comparing quaternion methods against a known dataset to make sure the math is correct

Based on https://mariogc.com/post/angular-velocity-quaternions/
RepoIMU: https://github.com/agnieszkaszczesna/RepoIMU/tree/main
"""

import numpy as np
import matplotlib.pyplot as plt
import pytransform3d.rotations as rt

# Method from the cited website
# I also tested this against AHRS and it seems to match up
def angular_velocities(q1, q2, dt):
    return (2 / dt) * np.array(
        [
            q1[0] * q2[1] - q1[1] * q2[0] - q1[2] * q2[3] + q1[3] * q2[2],
            q1[0] * q2[2] + q1[1] * q2[3] - q1[2] * q2[0] - q1[3] * q2[1],
            q1[0] * q2[3] - q1[1] * q2[2] + q1[2] * q2[1] - q1[3] * q2[0],
        ]
    )

# This data file seemed to have a nice set of values with good variation and signal/noise ratio
# This path will need to be modified if this script is replicated on another machine
data_file = "/home/dan/software/RepoIMU/TStick/TStick_Test07_Trial1.csv"

# This is the method from the cited page
array = np.loadtxt(data_file, dtype=float, delimiter=";", skiprows=2)
times = array[:, 0]
quaternions = array[:, 1:5]
gyroscopes = array[:, 8:11]
angvel = np.zeros_like(gyroscopes)
for i in range(1, len(angvel)):
    dt = times[i] - times[i - 1]
    angvel[i] = angular_velocities(quaternions[i - 1], quaternions[i], dt)

# Testing with pytransforms
dt = 0.01  # Constant, observed based on the reference data
angvel_2 = rt.quaternion_gradient(quaternions, 0.01)

# Plotting each method against the ground truth info from the dataset
colors = ["red", "green", "blue"]
components = ["wx", "wy", "wz"]
for i in range(3):
    plt.plot(
        times,
        gyroscopes[:, i],
        label=f"{components[i]}: Truth",
        color=colors[i],
    )
    plt.plot(
        times,
        angvel[:, i],
        label=f"{components[i]}: Theirs",
        color=colors[i],
        linestyle="dashed",
    )
    plt.plot(
        times,
        angvel_2[:, i],
        label=f"{components[i]}: Pytransform",
        color=colors[i],
        linestyle="dotted",
    )
# These limits made it easier to compare the data in a "good" area of the plot
# Can always zoom out and view more areas
plt.xlim(9, 13)
plt.ylim(-5, 4)
plt.legend()
plt.show()

[danielpmorton]

Note that the method from AHRS seems to work, but I originally switched to pytransform3d because of the central differencing. I may try to write a similar version that uses central differencing but I'm unsure if that was the cause of the issue in the first place

[AlexanderFabisch]

Hi @danielpmorton ,

that's very interesting. I currently cannot tell what the difference between the two approaches are. I'll look into this. Looks like some of the components are sometimes flipped, but that might just be coincidence.

I extracted the implementation from this library, which implements Quaternion DMPs. The inverse operation to quaternion_gradient is quaternion_integrate.

[danielpmorton]

It's interesting because there are some regions of the plot where it tracks exactly, some areas where it's flipped, and some areas where there is no discernable trend. I was originally wondering if it was due to quaternion double-cover, but I don't believe that's the case. And all of their quaternions in the reference data seem to be (very close to) normalized. I'll look into this some more as well

[AlexanderFabisch]

I was originally wondering if it was due to quaternion double-cover, but I don't believe that's the case.

It's more likely that there is an ambiguity in the axis-angle representation. We can also normalize to [0, pi) with this function.

pytransform3d's quaternion_gradient computes

$$\frac{1}{2 \Delta t}\log \left( q(t + \Delta t) \cdot \overline{q}(t - \Delta t) \right)$$

In code:

def quaternion_gradient(Q, dt=1.0):
    Q = check_quaternions(Q)
    Qd = np.empty((len(Q), 3))
    Qd[0] = compact_axis_angle_from_quaternion(
        concatenate_quaternions(Q[1], q_conj(Q[0]))) / dt
    for t in range(1, len(Q) - 1):
        # divided by two because of central differences
        Qd[t] = compact_axis_angle_from_quaternion(
            concatenate_quaternions(Q[t + 1], q_conj(Q[t - 1]))) / (2.0 * dt)
    Qd[-1] = compact_axis_angle_from_quaternion(
        concatenate_quaternions(Q[-1], q_conj(Q[-2]))) / dt
    return Qd

Or as forward differences:

def quaternion_gradient(Q, dt=1.0):
    Q = check_quaternions(Q)
    Qd = np.empty((len(Q), 3))
    for t in range(1, len(Q)):
        Qd[t] = compact_axis_angle_from_quaternion(
            concatenate_quaternions(Q[t], q_conj(Q[t - 1]))) / dt
    return Qd

[danielpmorton]

That does sound right. I wasn't aware that axis/angle had any ambiguities (other than when the angle is 0), but it would make sense since the logarithmic map for the quaternions should work

For reference, here's the function I just made for my repo that applies central differencing to the earlier method I shared -- in case you want to test against this. It seems to be working on my side

def quats_to_angular_velocities(
    quats: np.ndarray, dt: Union[float, npt.ArrayLike]
) -> np.ndarray:
    """Determines the angular velocities of a sequence of quaternions, for a given sampling time

    Based on AHRS (Attitude and Heading Reference Systems): See ahrs/common/quaternion.py

    Args:
        quats (np.ndarray): Sequence of WXYZ quaternions, shape (n, 4)
        dt (Union[float, np.ndarray]): Sampling time(s). If passing in an array of sampling times,
            this must be of length n

    Returns:
        np.ndarray: Angular velocities (wx, wy, wz), shape (n, 3)
    """
    ws = quats[:, 0]
    xs = quats[:, 1]
    ys = quats[:, 2]
    zs = quats[:, 3]
    n = quats.shape[0]  # Number of quaternions

    # This uses a new central differencing method to improve handling at start/end points
    dw = np.zeros((n, 3))
    # Handle the start
    dw[0, :] = np.array(
        [
            ws[0] * xs[1] - xs[0] * ws[1] - ys[0] * zs[1] + zs[0] * ys[1],
            ws[0] * ys[1] + xs[0] * zs[1] - ys[0] * ws[1] - zs[0] * xs[1],
            ws[0] * zs[1] - xs[0] * ys[1] + ys[0] * xs[1] - zs[0] * ws[1],
        ]
    )
    # Handle the end
    dw[-1, :] = np.array(
        [
            ws[-2] * xs[-1] - xs[-2] * ws[-1] - ys[-2] * zs[-1] + zs[-2] * ys[-1],
            ws[-2] * ys[-1] + xs[-2] * zs[-1] - ys[-2] * ws[-1] - zs[-2] * xs[-1],
            ws[-2] * zs[-1] - xs[-2] * ys[-1] + ys[-2] * xs[-1] - zs[-2] * ws[-1],
        ]
    )
    # Handle the middle range of quaternions
    # Multiply by a factor of 1/2 since the central difference covers 2 timesteps
    dw[1:-1, :] = (1 / 2) * np.column_stack(
        [
            ws[:-2] * xs[2:] - xs[:-2] * ws[2:] - ys[:-2] * zs[2:] + zs[:-2] * ys[2:],
            ws[:-2] * ys[2:] + xs[:-2] * zs[2:] - ys[:-2] * ws[2:] - zs[:-2] * xs[2:],
            ws[:-2] * zs[2:] - xs[:-2] * ys[2:] + ys[:-2] * xs[2:] - zs[:-2] * ws[2:],
        ]
    )
    # If dt is scalar, broadcasting is simple. If dt is an array of time deltas, adjust shape for broadcasting
    if np.ndim(dt) == 0:
        return 2.0 * dw / dt
    else:
        if len(dt) != n:
            raise ValueError(
                f"Invalid dt array length: {len(dt)}. Must be of length {n}"
            )
        return 2.0 / (np.reshape(dt, (-1, 1)) * dw)

[AlexanderFabisch]

I got a nearly perfect fit with this modification of pytransform3d's version:

def quaternion_gradient(Q, dt=1.0):
    Q = rt.check_quaternions(Q)
    Qd = np.empty((len(Q), 3))
    for t in range(1, len(Q)):
        Qd[t] = rt.compact_axis_angle_from_quaternion(
            rt.concatenate_quaternions(rt.q_conj(Q[t - 1]), Q[t])) / dt
    return Qd

I switched from central differences to backward differences, but the main difference is that I changed the order from

Qd[t] = rt.compact_axis_angle_from_quaternion(
            concatenate_quaternions(Q[t], q_conj(Q[t - 1]))) / dt

to

Qd[t] = rt.compact_axis_angle_from_quaternion(
            rt.concatenate_quaternions(rt.q_conj(Q[t - 1]), Q[t])) / dt

That means the interpretation of the angular velocities is different. In pytransform3d's version we integrate the angular velocities through left-multiplication (global frame):

$$\omega(t) = \frac{1}{\Delta t}\log \left( q(t) \cdot \overline{q}(t - 1) \right)$$

$$q(t) = \exp(\Delta t \omega(t)) q(t-1)$$

In the modified version, which is close to your version, through right-multiplication (body-fixed frame):

$$\omega(t - 1) = \frac{1}{\Delta t}\log \left( \overline{q}(t - 1) \cdot q(t) \right)$$

$$q(t) = q(t-1) \exp(\Delta t \omega(t-1))$$

I hope that makes sense.

[danielpmorton]

This looks great! And interesting note about the body-fixed vs global frames, I hadn't considered that