Converting Quaternions to Euler (Sparkfun LSM6DSV16X)

[kriista]

So I've got a Sparkfun LSM6DSV16X (6DOF IMU board with sensor fusion) and although the Sparkfun library doesn't seem to output the sensor fusion quaternions (as far as I've been able to tell from the examples), there's another library (by stm32duino) that does output quaternions.

There's even a handy web visualizer that works with it:
https://adafruit.github.io/Adafruit_WebSerial_3DModelViewer/

Now the issue I'm having is that I want to get an Euler (pitch/roll/yaw) representation of the orientation of the sensor rather than the Quaternion but I'm a bit confused by the specifications.

The datasheet says:

Game rotation vector: X, Y, and Z axes (vector part of the quaternion) are stored in half-precision floating- point format, where w (scalar part of the quaternion) is computed in software after reading the data from the FIFO, since the game rotation vector is a unit quaternion.

Which would lead me to believe that it's just a 3d vector output, but the code itself is outputting 4d:

      AccGyr.FIFO_Get_Tag(&tag);
      if (tag == 0x13u) {
        AccGyr.FIFO_Get_Rotation_Vector(&quaternions[0]);

        // Print Quaternion data
        Serial.print("Quaternion: ");
        Serial.print(quaternions[3], 4);
        Serial.print(", ");
        Serial.print(quaternions[0], 4);
        Serial.print(", ");
        Serial.print(quaternions[1], 4);
        Serial.print(", ");
        Serial.println(quaternions[2], 4);

That in-and-of-itself isn't too bad, but where I'm getting really confused is that I know that quaternions can come in different orders (w, x, y, z or x, y, z, w), furthermore, the order the quaternions are being print-ed here is different too (3, 0, 1, 2). From what I can tell, there's no mention of what order the LSM6DSV16X is outputting the data as.

My math isn't good enough to be able to tell what the order is just by looking at the numbers.

Now the main reason I'm writing this is because I found another forum post on here for converting from quaternions to Euler angles, but this only works with one of the orders.

float yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
float pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
float roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);

Does anyone have experience with using the LSM6DSV16X with Euler outputs or know maths well enough to know what order things are coming in as in the sensor fusion library?

[jremington]

In order to derive the equations for Euler angles, you need to know how the quaternion is defined according to the sensor axes X, Y and Z, and how those axes are oriented in the standard "real world" coordinate system.

For example, when the sensor indicates that you are pointing due North, and are level, which sensor axis is pointing due North and which axis is pointing up?

[kriista]

Thanks for the quick response!

@jremington

In order to derive the equations for Euler angles, you need to know how the quaternion is defined according to the sensor axes X, Y and Z, and how those axes are oriented in the standard "real world" coordinate system.

That's the thing, I'm not entirely sure based on what I'm reading from the datasheets/code.

@jremington

For example, when the sensor indicates that you are pointing due North, and are level, which sensor axis is pointing due North and which axis is pointing up?

Ok, trying to do this via deduction, I'm oriented the board facing North and with a flat roll/pitch and that gives me this ("rabbit facing away"):

If I point the sensor South, trying to keep everything else stable, I get this ("rabbit facing towards"):

In looking at the numbers, it seems like the only value that has changed a significant amount is the 4th one. So would that be the Z then? Meaning the quaternions are w, x, y, z?

For cross-checking here are some more rabbit orientations.

While remaining pointing South, I've tilted the roll:

Now, all but the first value change here. I don't know if that's because of how quaternions are encoded, or if I was just that sloppy with my rotation (doesn't look like it).

And lastly, trying to keep the pitch/roll flat (tricky while looking at it), but rotating the yaw:

//////////////////////////////////////////////////////////////////////////////

Does this point towards the w, x, y, z order?

And a couple other follow up questions.
Are quaternions explicitly between -1. and 1.?
Should the Euler output be in degrees?

I initially tried plugging the code in assuming the order was correct like so:

  if (fifo_samples > 0) {
    for (int i = 0; i < fifo_samples; i++) {
      AccGyr.FIFO_Get_Tag(&tag);
      if (tag == 0x13u) {
        AccGyr.FIFO_Get_Rotation_Vector(&quaternions[0]);

        // Print Quaternion data
        Serial.print("Quaternion: ");
        Serial.print(quaternions[3], 4);
        Serial.print(", ");
        Serial.print(quaternions[0], 4);
        Serial.print(", ");
        Serial.print(quaternions[1], 4);
        Serial.print(", ");
        Serial.println(quaternions[2], 4);

        float yaw   = atan2(2.0f * (quaternions[1] * quaternions[2] + quaternions[0] * quaternions[3]), quaternions[0] * quaternions[0] + quaternions[1] * quaternions[1] - quaternions[2] * quaternions[2] - quaternions[3] * quaternions[3]);
        float pitch = -asin(2.0f * (quaternions[1] * quaternions[3] - quaternions[0] * quaternions[2]));
        float roll = atan2(2.0f * (quaternions[0] * quaternions[1] + quaternions[2] * quaternions[3]), quaternions[0] * quaternions[0] - quaternions[1] * quaternions[1] - quaternions[2] * quaternions[2] + quaternions[3] * quaternions[3]);

        Serial.print("Eural: ");
        Serial.print(yaw);
        Serial.print(", ");
        Serial.print(pitch);
        Serial.print(", ");
        Serial.println(roll);

That gives me results that look like this (incorrect looking to me?):

20:34:37.909 -> Quaternion: 0.8488, 0.1202, 0.4954, -0.1404
20:34:37.909 -> Eurel: 3.01, -1.06, -0.25
20:34:37.941 -> Quaternion: 0.8488, 0.1202, 0.4954, -0.1404
20:34:37.941 -> Eurel: 3.01, -1.06, -0.25
20:34:37.941 -> Quaternion: 0.8488, 0.1202, 0.4954, -0.1404
20:34:37.941 -> Eurel: 3.01, -1.06, -0.25
20:34:37.941 -> Quaternion: 0.8488, 0.1202, 0.4954, -0.1405
20:34:37.941 -> Eurel: 3.01, -1.06, -0.25

[jremington]

You seem to be mixing three libraries, which may have different and conflicting definitions for mapping the sensor axes to the Earth axes. I doubt you will have much fun trying to guess what those are.

The Sparkfun library is just a device driver that controls sensor settings and returns the accel/gyro data.

A sensor fusion library will interpret those data and provide a quaternion and possibly, Euler angles, with respect to some arbitrary standard orientation (and there are many in use).

A display library makes additional assumptions about the source of the data and the screen coordinates.

In order to understand the output of the sensor fusion algorithm, you need to look at the documentation and/or code to determine how the sensor axes are defined and used.

[kriista]

@jremington

You seem to be mixing three libraries, which may have different and conflicting definitions for mapping the sensor axes to the Earth axes. I doubt you will have much fun trying to guess what those are.

In the code above, there's only one library running, the one by stm32duino. The Adafruit visualizer is referenced in that code so the outputs are ordered/scaled to work with that.

All the screenshots and output printing above is from the library example LSM6DSV16X_Sensor_Fusion.ino.

I've mentioned the other libraries just to say/show what I've experimented with.

@jremington

The Sparkfun library is just a device driver that controls sensor settings and returns the accel/gyro data.

A sensor fusion library will interpret those data and provide a quaternion and possibly, Euler angles, with respect to some arbitrary standard orientation (and there are many in use).

The reason I chose the LSM6DSV16X is because it is supposed to have on-board sensor fusion (via the datasheet). I don't know why the Sparkfun library doesn't seem to give that as an output option, only giving the accel/gyro data directly.

@jremington

In order to understand the output of the sensor fusion algorithm, you need to look at the documentation and/or code to determine how the sensor axes are defined and used.

And that's where I'm kind of stuck. The LSM6DSV16X datasheet just says this:

Game rotation vector: X, Y, and Z axes (vector part of the quaternion) are stored in half-precision floating- point format, where w (scalar part of the quaternion) is computed in software after reading the data from the FIFO, since the game rotation vector is a unit quaternion.

And the stm32duino library makes no mention of any order.

[jremington]

I hadn't looked at the device data sheet, and just now did.

The illustration copied below provides just about everything you need to know about how the sensor axes are oriented, and you can then compare the chip orientation on the board to board markings.

Since that is a 6DOF sensor, the yaw angle will be relative to some arbitrary starting orientation, or likely, you can "reset" yaw to zero with software. Yaw will be defined solely by integrating the gyro rates.

Very likely, if the board is level and is started up, the unit quaternion (1,0,0,0) or (w,x,y,z) will be defined according to the axial directions in the figure below (Z axis vertical, Y axis pointing toward yaw = 0, pseudo North direction).

You need a magnetometer or other horizon reference to define absolute North.

[jremington]

@kriista

Which would lead me to believe that it's just a 3d vector output, but the code itself is outputting 4d:

It is just a 3D vector on the chip, storing only the imaginary components x, y, z. Since those are normalized, the real component w = sqrt(1 - (x^2 + y^2 + z^2))

[kriista]

Thanks so much for taking a detailed look!
The math-ier side of this stuff shoots comfortably over my head!

@jremington

Since that is a 6DOF sensor, the yaw angle will be relative to some arbitrary starting orientation, or likely, you can "reset" yaw to zero with software. Yaw will be defined solely by integrating the gyro rates.

That was my plan, to just code a tare function downstream so I don't have to worry about orientation when booting it up.

@jremington

It is just a 3D vector on the chip, storing only the imaginary components x, y, z. Since those are normalized, the real component w = sqrt(1 - (x^2 + y^2 + z^2))

Ah right, didn't realize that part of the transform was so simple (for 3d-quaternion->4d-quaternion).

Where this throws me is that the datasheet says it's just the XYZ but the library (and example sketch) is outputting 4d:

        AccGyr.FIFO_Get_Rotation_Vector(&quaternions[0]);

        // Print Quaternion data
        Serial.print("Quaternion: ");
        Serial.print(quaternions[3], 4);
        Serial.print(", ");
        Serial.print(quaternions[0], 4);
        Serial.print(", ");
        Serial.print(quaternions[1], 4);
        Serial.print(", ");
        Serial.println(quaternions[2], 4);

I imagine based on what you've said, that the library itself is just applying that sqrt function to the xyz to generate the w, and then outputs that first (at least in the printing order).

I'm guessing the function I found on that other forum post is assuming xyzw (rather than wxyz as it appears to be coming out of the sketch.

/////////////////////////////////////////////////////////////////////////////////////

A small tangential question.

If I'm only really interested in the pitch/roll output, is using 9DOF (for drift compensation) important or would 6DOF be fine in that case?

As far as I understand it, the 9DOF fusion mainly applies to yaw/rotational drift, but I could be wrong there.

For context: I also ordered a 9DOF board but it's at least 4x the size of this one. Not normally an issue, but my intended use case is mounting this on an acoustic cymbal, so the smaller the better, in terms of not impacting the vibration of the cymbal itself.

[jremington]

wxyz as it appears to be coming out of the sketch

That seems to be the most popular order, but some people write w last.

For pitch and roll in the static case, all you need is an accelerometer. See How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing-DFRobot

For the dynamic case, the accelerometer and gyro data must be combined to estimate pitch and roll. The gyro alone defines the yaw angle, and since that is an integrated rate term, the origin (yaw = 0) is arbitrary, and yaw will drift.

[kriista]

In my case, I imagine this would be the static case as even though the cymbal will wobble across two axes (pitch/roll), it's always going to stay put and return to equilibrium?

My overall thinking here is to use the movement data to augment the normal audio analysis from a cymbal to create a more realistic/complex electronic component that is modulated by the movement of the cymbal (much like what happens in reality as the cymbal wobbles around).

@jremington

For pitch and roll in the static case, all you need is an accelerometer. See How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing-DFRobot

Oooh very handy!

Since it just takes the accelerometer data, I can use the much friendly/documented Sparkfun library instead.

I just popped that snippet into the code and left it running for like 10min and there's no drift or anything as far as I can tell (other than some general +/- 0.03 noise).

[jremington]

@kriista

there's no drift

This is expected! It is the gyro that drifts.

Average a few accelerometer values to reduce the noise.

[kriista]

@jremington

Average a few accelerometer values to reduce the noise.

Ultimately I'll do something like this, but want to be mindful of not adding too much latency since it will be in a musical context. I tend to use ResponsiveAnalogRead a bunch, but wanted to get things up and running before ironing out details like that.

//////////////////////////////////////

Ok played with this in context today (i.e. on a physical cymbal).

The pitch/roll computation from the tilt-sensing robot code you posted worked well, worked really well on the cymbal (rescaling/tare-ing it to zero out the starting position).

But I did encounter something quite unexpected. Or rather, unintended.

Striking the cymbal would create a large spike in the accelerometer data even if the "pitch" and "roll" stayed relatively constant.

Here's a quick-n-dirty video showing what I mean:

6DOF IMU on Cymbal test

(This is just a proof-of-concept setup, the IMU would be mounted under the bell so it doesn't interfere with the cymbal, and it would ultimately be doing something more interesting than filtering noise, this is just to sonify the data to show what's going on.)

//////////////////////////////////////

I tried working with the gyro data directly instead, but for the life of me I couldn't figure out what range they are meant to be in, it seemed to vary at least +/- 50k, which seems wild.

[jremington]

@kriista

Striking the cymbal would create a large spike in the accelerometer data even if the "pitch" and "roll" stayed relatively constant.

That is not surprising. Pitch and Roll use a ratio, so the total acceleration doesn't matter much.

[kriista]

@jremington

That is not surprising. Pitch and Roll use a ratio, so the total acceleration doesn't matter much.

Ah that would make sense. I'm not visualizing the raw accel data, but I imagine it shows the huge spike from the energy of the strike (ala tap detection).

I suppose the sensor fusion would suffer from a similar "problem"?

Which would leave just the raw gyro data to content with?

[kriista]

Ok as an addendum, the best results, and effectively the solution here, is that I used the stm32duino library and specifically the LSM6DSV16X_Sensor_Fusion.ino example and converted the output of the library's quaternions (which come as XZYW by default) to WXYZ, then used a normal quaternion to euler transformation to turn it back into pitch/roll/yaw.

Relevant bits of code are:

  // Check the number of samples inside FIFO
  if (AccGyr.FIFO_Get_Num_Samples(&fifo_samples) != LSM6DSV16X_OK) {
    Serial.println("LSM6DSV16X Sensor failed to get number of samples inside FIFO");
    while (1);
  }

  // Read the FIFO if there is one stored sample
  if (fifo_samples > 0) {
    for (int i = 0; i < fifo_samples; i++) {
      AccGyr.FIFO_Get_Tag(&tag);
      if (tag == 0x13u) {
        AccGyr.FIFO_Get_Rotation_Vector(&quaternions[0]);

//void quaternion_to_euler_angle(float quaternions[4], float &yaw, float &pitch, float &roll) {
    float w = quaternions[3];
    float x = quaternions[0];
    float y = quaternions[1];
    float z = quaternions[2];
    
    // Yaw (z-axis rotation)
    float yaw = atan2(2.0f * (x * y + w * z), w * w + x * x - y * y - z * z);
    
    // Pitch (y-axis rotation)
    float pitch = -asin(2.0f * (x * z - w * y));
    
    // Roll (x-axis rotation)
    float roll = atan2(2.0f * (w * x + y * z), w * w - x * x - y * y + z * z);

This not only avoids the weird spike in the accelerometer-derived versions of pitch/roll linked here, but it appears to be much better behaved data too. Way way less noisy and without the lag I was getting with the Sparkfun library.

Sadly the code isn't as pretty/documented, but it appears to be working well at this point.

Here's a vid of it in action (again, at a proof-of-concept stage):

Cymbal 6DOF IMU Filter Test

This is using onset detection to trigger a burst of noise, then applying filtering/phasing based on the pitch/roll movement of the acoustic cymbal. The acoustic cymbal is panned hard left, and the filtered noise is panned hard right.

The IMU is mounted underneath the cymbal in the bell area.