Difference between the two quaternions -- Euler Components

Question

I am working on a wearable device using the Invensense 9D (Accel+Gyro+Compass) which computes a quaternion for real-time orientation, using the embedded Digital Motion Processor running a Kalman filter. Sadly -- the DMP code is not something that Invensense allows end-users access to.

The resulting quaternions in our application suffer from some drift. But we've been able to work within that by taking the differences between the Euler angles over short time-frames. But Euler angles are obviously less than desirable because of their inherent issues (gimbal lock, etc).

Where I'm struggling is computing the difference between two quaternions -- which 'should' be Diff = q2 * conj(q1) -- then computing the Pitch and Roll changes required to get from q1 to q2. That's where my brain freezes.

Appreciate any suggestions/pointers.

UPDATE :

I do indeed need to display the angular differences -- in both pitch and roll -- to a user so that they can understand the orientation difference between the two orientations.

FS.Roll <<- atan2((2.0*(FS.Quat.Q0*FS.Quat.Q1 + FS.Quat.Q2*FS.Quat.Q3)), (1.0 - 2.0*(FS.Quat.Q1^2 + FS.Quat.Q2^2))) * M_PI;
FS.Pitch <<- asin((2.0*(FS.Quat.Q0*FS.Quat.Q2 - FS.Quat.Q3*FS.Quat.Q1))) * M_PI;

MP.Roll <<- atan2((2.0*(MP.Quat.Q0*MP.Quat.Q1 + MP.Quat.Q2*MP.Quat.Q3)), (1.0 - 2.0*(MP.Quat.Q1^2 + MP.Quat.Q2^2))) * M_PI;
MP.Pitch <<- asin((2.0*(MP.Quat.Q0*MP.Quat.Q2 - MP.Quat.Q3*MP.Quat.Q1))) * M_PI;

If I take the differences between the FS.Pitch and MP.Pitch and the corresponding FS.Roll and MP.Roll -- I get exactly what I'm after.

But I'd like to reduce the inverse trig calcs on the embedded Cortex-M0 MCU -- as well as avoiding Gimbal Lock for each conversion -- so ideally I'd like to get the Quaternion 'Difference' between the two and decompose it into the relative Pitch and Roll components.

When I try your suggestion -- it yields different results than the Euler math. I understand that there are number of different rotation sequences to get from a Quaternion to an Euler angle. I've tried over a dozen of them, none of which yields the same answer as taking the difference of the Euler angles from the individual Quaternions.

It feels like I'm missing something obvious -- but I'm just not seeing it.

Thanks!

Gouda Gouda · Accepted Answer · 2015-02-01T06:20:11

For two quaternion orientations p and q the quaternion that would take p into the q frame is r = p* ⊗ q.

Then when you multiply: p ⊗ r, quaternions are associative so:

p ⊗ r = p ⊗ (p* ⊗ q) = (p ⊗ p*) ⊗ q = q.

To get the Euler angles of r, you should use a library. Here's my Python code for it, if it helps:

def quat2eulerXYZ(q):
  r = atan2(2*q[0][0]*q[1][0] - 2*q[2][0]*q[3][0], q[0][0]**2 - q[1][0]**2 - q[2][0]**2 + q[3][0]**2)
  p = asin(2*q[0][0]*q[2][0] + 2*q[1][0]*q[3][0])
  y = atan2(2*q[0][0]*q[3][0] - 2*q[1][0]*q[2][0], q[0][0]**2 + q[1][0]**2 - q[2][0]**2 - q[3][0]**2)
  return (r, p, y)

If your AHRS system is drifting, it is probably the magnetometer suffering from interference which may be solved by calibrating for soft/hard iron interference. If your AHRS system is in close proximity or fastened to any ferrous metals, try it without them. If you try this outdoors, clear of ferrous metals, I would be surprised if it did not work.

You shouldn't need to convert to Euler angles once you have a quaternion, unless it is for display purposes. If you elaborate on what you are doing with the Euler angles I may be of more assistance.

Difference between the two quaternions -- Euler Components

1 Answers