qqmul - wrong calculation?

[bttner]

Hey. Today I was comparing the Eigen library with yours - cmath3d - and got stuck at the implementation of qqmul which is suppose to calculate the multiplication of two quaternions such that qvrot(qqmul(q, p), v) == qvrot(q, qvrot(p, v)) where q, p are quaternions and v is a vector.

Concrete, I figured out that the result of qqmul is different from what Eigen outputs. However, when I adjust the equations in qqmul according to link1 (or link2) I get a similar result to what Eigen gave me; the changes can be seen in qqmul2 in the code below.

Then I thought that maybe the difference comes from the above condition and wrote the following code

// main.c
#include <stdio.h>
#include <stdlib.h>
#include "math3d.h"

static inline struct quat qqmul2(struct quat q, struct quat p)
{
    // i.e., https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm
    float x = q.w * p.x - q.z * p.y + q.y * p.z + q.x * p.w;
    float y = q.z * p.x + q.w * p.y - q.x * p.z + q.y * p.w;
    float z = -q.y * p.x + q.x * p.y + q.w * p.z + q.z * p.w;
    float w = -q.x * p.x - q.y * p.y - q.z * p.z + q.w * p.w; // = old w

    return mkquat(x, y, z, w);
}

static inline bool compare(struct vec _v1, struct vec _v2)
{
    return abs(_v1.x - _v2.x) <= 1e-3 && abs(_v1.y - _v2.y) <= 1e-3 && abs(_v1.z - _v2.z) <= 1e-3;
}

int main()
{
    // Initializations.
    struct quat const q = qnormalize(mkquat(0, 1, -3, 2));
    struct quat const p = qnormalize(mkquat(4, 5, 6, 3));
    struct vec const v = mkvec(1, 2, -1);

    // Original qqmul vs. new version
    struct quat res0 = qqmul(q, p); // [x,y,z,w] = [-0.374653757, 0.720488012, 0.201736659, 0.547570825]
    struct quat res0n = qqmul2(q, p); // [x,y,z,w] = [0.835766077, 0.0288195536, -0.0288195163, 0.547570825]

    // Original version.
    // Proof of claim qvrot(qqmul(q, p), v) == qvrot(q, qvrot(p, v)).
    struct vec res1 = qvrot(qqmul(q, p), v);
    struct vec res2 = qvrot(q, qvrot(p, v));
    printf("Original: %i\n", compare(res1, res2)); // Original: 0 := false

    // New version of qqmul, namely qqmul2.
    // Proof of claim qvrot(qqmul2(q, p), v) == qvrot(q, qvrot(p, v)).
    struct vec res1n = qvrot(qqmul2(q, p), v);
    struct vec res2n = qvrot(q, qvrot(p, v));
    printf("New: %i\n", compare(res1n, res2n)); // New: 1 := true

    return 0;
}

which can be compiled via

gcc -g main.c -lm

As a result, when using qqmul the above condition does not hold. It does hold however when using qqmul2.

Now I am wondering if I missed something or if the current implementation is wrong?

Thank you in advance.

[jpreiss]

Somehow I missed this issue when it was created - apologies.

A lot of the quaternion code in this library, including qqmul, implements formulae from the paper

Indirect Kalman Filter for 3D Attitude Estimation
Nikolas Trawny and Stergios I. Roumeliotis
Technical Report, Department of Computer Science and Engineering, University of Minnesota
March 2005

This paper uses the JPL quaternion convention, which is designed to preserve the property
qvrot(qqmul(p, q), v) == qvrot(p, qvrot(q, v)).

However, since that paper doesn't give an explicit formula for quaternion-vector rotation, I copied it from a different source instead, that used the Hamiltonian convention. This was a big mistake that would have been caught if the project had adequate unit testing in the beginning.

Unfortunately there is no way to fix this problem without breaking the API. I want to stick with JPL, so qvrot (and qvectovec) need to change.

I am inclined to break the API because the primary user of this library (https://github.com/bitcraze/crazyflie-firmware) doesn't even use qvrot or qvectovec.

Thoughts?