Pitch behaving abnormally?

[simon-hauser]

Hi,

I'm having an issue with my IMU and Fusion that gives me a headache, so I'm looking for suggestions from everywhere. My hardware is a XIAO nrf52840 BLE sense, and I want to use the internal 6-axis IMU (gyro+accelerometer) together with Fusion. I'm using code mainly written by somebody else with the same hardware, but overall it's basically your simple example. The output of each single measurement itself looks fine as far as I can tell: accelerometer going from -1 to 1 for each axis with no crosstalk, gyro going from e.g. -50 to 50 for a moderate turning rate for each axis individually. Now I do

const FusionVector gyroscope = { (xiaoIMU.readFloatGyroX() - 0.35f), (xiaoIMU.readFloatGyroY() + 2.65f), (xiaoIMU.readFloatGyroZ() - 0.25f) };
const FusionVector accelerometer = { xiaoIMU.readFloatAccelX(), xiaoIMU.readFloatAccelY(), xiaoIMU.readFloatAccelZ() };
FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, IMU_SAMPLE_RATE);
const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
Serial.print(euler.angle.roll);
Serial.print(",");
Serial.print(euler.angle.pitch);
Serial.print(",");
Serial.println(euler.angle.yaw);  

with some basic calibration applied to the gyro. I attached pictures of this output for different 180° rotations. The first one is roll, and I turn the IMU from 0° to 180°. Everything looks normal, i.e. pitch and yaw stay at 0° and roll increases to 180° as expected. I'm doing this in my hand, so small deviations can occur. The second picture is yaw, likewise showing the expected behavior. But in the third picture, when rotating around pitch from 0° to 180°, this is the output I get. For the first 90°, it kind of looks fine, but then instead of pitch increasing to 180°, it decreases again to 0° while the other two axes both rapidly go to -180°. The fourth picture is the output of pitch going from 0° to -180°, where a similar thing happens.

From what I understand, the euler angles is the orientation output to a fixed reference frame, which is exactly what I need. In this case, I don't think I'm crossing some singularity that causes this. But please let me know if this is the correct output for the cases I describe. If yes, I'll have to read up on the euler angles output. If no, I would appreciate any suggestion as to what could cause this and where to look into. I was hoping that you may have seen such a behavior before and have some valuable insight.

Thank you very much for your help! Best regards!

[simon-hauser]

Hm, looks like my previous statement of not crossing a singularity is wrong. I'm reading that a singularity happens exactly where I see it, at pitch = 90° or pi/2, and I'm guessing the smooth transition I have in the output is produced by an internal filtering so the actual singularity is smeared out a bit. Is it possible to sharpen the smoothing to have the pitch angle valid until closer to +-90°? I think the internal quaternion representation should still be correct, but this is "just" the drawback of mapping them to the euler angles, right? That complicates things a bit in my project. I wanted to put my IMU in an arbitrary orientation, get the angles, and then have the IMU report me if the orientation deviates from the set one by some threshold roll-pitch-yaw degrees. But that wouldn't work if one puts the initial orientation close to the singularity. I see three possibilities: either switch the angles such that pitch is an angle that could be restricted to roughly +-60° (which may be possible), or use two sets of euler angles and switch between them at certain angular values, or figure out if I can use the quaternion representation directly to do what I want. Please let me know if I'm on the right track here.

[xioTechnologies]

You are correct that the issue is the singularity that occurs in the Euler angle representation as the roll angle approaches ±90°. The smoothing is the result of the imperfect numerical data, not filtering. Euler angles may be unnecessary for your application. Please can you describe your application, and the problem you are trying to solve.

[simon-hauser]

I couldn't follow the argument of the imperfect numerical data. I understand now that there is a singularity in pitch = +-90°, but I would think that the angles within this boundary will be represented correctly. In the plots, one can see that an issue with roll and yaw arises at roughly pitch = +- 60°, which is still relatively far from the singularity. Is it that the numerical values are already hard to deal with by the microcontroller, i.e. the precision is not enough to resolve closer to the singularity?

I have two similar applications. One is that I want the IMU to report if an angle crosses a specific threshold. For this, the absolute orientation is required in some form. The other application is that I want to put the IMU in a certain orientation, and then internally fix this orientation, such that the IMU reports back if it has been rotated out of the specified orientation. For this, theoretically only relative angles are needed. However, due to drift and noise, it could be that over time the IMU orientation can significantly differ from the specified one without the IMU reporting the difference. So right now I think also absolute orientation is needed. Do you see another way of solving these applications besides Euler angles? Inputs are appreciated. As I wrote, I could try to limit both applications such that pitch can only be between +-60° and still use Euler angles. Or maybe there is a clever way to use multiple bases of Euler angles and switch between them at appropriate angles to cover the whole space. Or maybe I overlooked something what you know could be a solution? Thanks for your help!

[xioTechnologies]

If you generate quaternations describing a pure rotation in pitch from 60° to 90°, and convert these to Euler angles using FusionQuaternionToEuler() then you will see no change in roll or yaw. You can demonstrate this for yourself using the following code.

import imufusion
import matplotlib.pyplot as pyplot
import numpy

pitch = numpy.linspace(numpy.pi/3, numpy.pi/2, 100)

roll = numpy.empty_like(pitch)
yaw = numpy.empty_like(pitch)

for index, angle in enumerate(pitch):
    w = numpy.cos(angle / 2)
    x = 0
    y = numpy.sin(angle / 2)
    z = 0

    euler = imufusion.Quaternion(numpy.array([w, x, y, z])).to_euler()

    roll[index] = euler[0]
    pitch[index] = euler[1]
    yaw[index] = euler[2]

pyplot.plot(roll, "tab:red", label="Roll")
pyplot.plot(pitch, "tab:green", label="Pitch")
pyplot.plot(yaw, "tab:blue", label="Yaw")
pyplot.xlabel("Index")
pyplot.ylabel("Angle (degrees)")
pyplot.grid()
pyplot.legend()
pyplot.show()

The changes you are observing must result from numerical imperfections such as:

• Approximation of the quaternion normalisation when FUSION_USE_NORMAL_SQRT is disabled. See line 117.
• Approximations of trigometric functions specific to your compiler/platform
• Imprecision of numerical data types specific to your compiler/platform

Euler angles are not necessary to detect an angular change in orientation. Any orientation can be described by a single rotation around a vector. This is known as an axis-angle representation. Quaternions have a simple relationship to axis-angle representation, as described in the diagram and equation below.

If you only care about the angular change described by a quaternion then you can obtain this from the qw element of the quaternion. The angle would be 0.5*acos(qw).

[simon-hauser]

Hi, Thanks for the additional info! I will look into your last point of using directly the quaternions, looks like this could solve my issue. Thanks for your help!