Efficient quaternion angular velocity

Question

I have an orientation expressed with a quaternion and an angular velocity expressed as either a quaternion or a number (radians per second around the original orientation).

Answer 1

For update of orientation , you require to multiply current orientation by delta rotation. This is comparable expensive operation with axis angle conversion.

Common way to represent angular velocity is "exponential map", the 3d vector parallel with rotation axis and magnitude of rotation velocity (radians per second). The conversion to delta rotation quaternion looks like

Quaternion deltaRotation(const Vector3& em, double deltaTime)
{
   Vector3 ha = em * deltaTime * 0.5; // vector of half angle
   double l = ha.norm(); // magnitude
   if (l > 0) {
      ha *= sin(l) / l;
      return Quaternion(cos(l), ha.x(), ha.y(), ha.z());
   } else {
      return Quaternion(1.0, ha.x(), ha.y(), ha.z());
   }
}

If your deltaTime is small and rotation speed is small, you can use approximation by 1st Taylor series multiplier. But you should normalize result quaternion to avoid numerical instability more often.

Quaternion deltaRotationAppx1(const Vector3& em, double deltaTime)
{
   Vector3 ha = em * deltaTime * 0.5; // vector of half angle
   return Quaternion(1.0, ha.x(), ha.y(), ha.z());
}

Answer 2

Here is the same first order Taylor series method as given in minorlogic's answer (using the JOML API):

public static Quaterniond integrateAngularVelocity(
        double avx, double avy, double avz, double dt) {
    double len = Math.sqrt(avx * avx + avy * avy + avz * avz);
    double theta = len * dt * 0.5;
    if (len > 1.0e-12) {
        double w = Math.cos(theta);
        double s = Math.sin(theta) / len;
        return new Quaterniond(avx * s, avy * s, avz * s, w);
    } else {
        return new Quaterniond(0, 0, 0, 1);
    }
}

The origin and meaning of this method is explained in the background section of A novel Quaternion integration approach for describing the behaviour of non-spherical particles by Facheng Zhao and Berend van Wachem (2013), and in significant detail in sections 4.5 (Time Derivatives) and 4.6 (Time-integration of Rotation Rates) of the excellent Quaternion kinematics for the error-state Kalman filter by Joan Solà (2017).

This is only a first-order Taylor series expansion of the time integral of angular velocity. A second-order correction is given in Eq 227 of Solà:

double avgAVX = 0.5 * (avPrev.x + avCurr.x);
double avgAVY = 0.5 * (avPrev.y + avCurr.y);
double avgAVZ = 0.5 * (avPrev.z + avCurr.z);
Quaterniond integral = IMUIntegration.integrateAngularVelocity(
        avgAVX, avgAVY, avgAVZ, dt);
Vector3d correction = avPrev.cross(avCurr, new Vector3d()).mul(dt * dt / 24.0);
Quaterniond qCorrection = new Quaterniond(
        correction.x, correction.y, correction.z, 0.0);
Quaterniond deltaOrientation = integral.add(qCorrection).normalize();

However the quaternion addition in the final step has no meaningful geometric interpretation, and increases the numerical error over the error introduced by integration at discrete time steps. Therefore, it is better to use a multiplication-only method, as introduced as the predictor–corrector direct multiplication (PCDM) method in Zhao and Wachem.

This method was improved upon in Improved quaternion-based integration scheme for rigid body motion by L. J. H. Seelen, J. T. Padding, and J. A. M. Kuipers (2016). If I'm reading the math right, and if you simplify by assuming there are no external torques or forces, and if you throw away the linear velocity components to focus only on angular velocity and rotation, and if you work only in local coordinates rather than switching back and forth between local and world coordinates (which the authors do in order to add in the contributions of external torques, velocities and forces), then you end up with a method that simply integrates the angular velocities between the previous and current IMU readings, and then between the current and next IMU readings, sampling at any desired resolution along these two interpolations, and applying the integrateAngularVelocity routine shown above with the smaller dt value induced by the integration sampling interval:

private static final int NUM_INTEGRATION_STEPS = 6; // Should be even

public static List<IMUIntegrated> integrateAngularVelocities(List<IMURaw> raw,
            double dt) {
    List<IMUIntegrated> integrated = new ArrayList<>();
    for (int i = 0; i < raw.size(); i++) {
        // Find average between curr and prev/next angular velocities
        IMURaw prevRawAV = i == 0 ? null : raw.get(i - 1);
        IMURaw currRawAV = raw.get(i);
        IMURaw nextRawAV = i == raw.size() - 1 ? null 
                : raw.get(i + 1);
        Vector3d prevAV = prevRawAV == null ? new Vector3d()
                : prevRawAV.getAngularVelocity();
        Vector3d currAV = currRawAV.getAngularVelocity();
        Vector3d nextAV = nextRawAV == null ? new Vector3d()
                : nextRawAV.getAngularVelocity();
        Vector3d prevAvgAV = prevAV.add(currAV, new Vector3d()).mul(0.5);
        Vector3d nextAvgAV = currAV.add(nextAV, new Vector3d()).mul(0.5);

        // Find integration interval
        double integrationDt = dt / NUM_INTEGRATION_STEPS;

        // Linearly interpolate and integrate angular velocities
        Quaterniond deltaOrientation = new Quaterniond();
        for (int j = 0; j <= NUM_INTEGRATION_STEPS; j++) {
            double frac;
            Vector3d startAV, endAV;
            if (j < NUM_INTEGRATION_STEPS / 2) {
                frac = (j / (double) (NUM_INTEGRATION_STEPS / 2));
                startAV = prevAvgAV;
                endAV = currAV;
            } else {
                frac = ((j - NUM_INTEGRATION_STEPS / 2)
                        / (double) (NUM_INTEGRATION_STEPS / 2));
                startAV = currAV;
                endAV = nextAvgAV;
            }
            // Linearly interpolate angular velocities
            Vector3d interpolatedAV = startAV.mul(1.0 - frac, new Vector3d())
                    .add(endAV.mul(frac, new Vector3d()));
            // Integrate to produce a quaternion
            deltaOrientation = integrateAngularVelocity(
                    interpolatedAV.x, interpolatedAV.y, interpolatedAV.z,
                    integrationDt)
                // Concatenate onto cumulative transformation
                .mul(deltaOrientation);
        }
        integrated.add(new IMUIntegrated(currRawAV.timestamp,
                deltaOrientation.normalize()));
    }
    return integrated;
}