Initial Roll is -179º instead of 0, and breaks linear acceleration

[oldspice7]

I have an MPU-9250 and I'm trying to use the Mahony filter to get better Pitch, Roll and linear acceleration. However, after I run mpu.calibrateAccelGyro() to calibrate the sensor (or even if I don't run it), while it's sitting still horizontally, Roll is -179º. It actually goes from -179º to +179º simply by tilting slightly. I'm not an expert on Roll, but while I understand its interval is [-180,+180º], is there a way to "calibrate" it to zero and also avoid this weird "wrap around" of -179º turning into +179º? Because linear acceleration will also be affected (mpu.getLinearAccZ() returns 19.6m/s^2 instead of 0m/s^2, when mpu.getAccZ() is 9.8m/s^2).

If I use QuatFilterSel.NONE, then the Roll is 0, as expected.

Could someone explain what I'm doing wrong? Thank you!

[oldspice7]

I believe this is related to #30. After investigating, I figured the initial Roll being around 179º (negative or positive) instead of 0 is because the library is using NED coordinates. They are interesting for Aircraft applications, as:

Coordinates to describe observations made from an aircraft are normally given relative to its intrinsic axes, but normally using as positive the coordinate pointing downwards, where the interesting points are located. Therefore, they are normally NED.

In other words, Roll with NED coordinates is supposed to be 0 only when the IMU sensor is pointing at the ground, and 180º when pointing at the sky. Naturally, this is great for pilots figuring out whether things took a turn for the worse (😛), but in my project, ENU seems more appropriate, i.e., I want that Roll be 0 when the IMU is laying flat and pointing at the sky.

Anyway, here's what I did.

@kriswiner, the author of the library this one is based on, claims switching from NED to ENU won't break the filters:

The sensor data need to be fed into the Madgwick or Mahony filter in a consistent way, usually NED but could be ENU.

Since to convert NED to ENU you swap X and Y axes and negate Z, I made the following change in the update() function (I ignored mn, me and md because they aren't used by the Mahony filter):

    bool update() {
        (...)
        quat_filter.update(ae, an, -ad, ge, gn, -gd, mn, me, md, q); //the 'd' in ad and gd thus becomes 'u' (i.e., 'down' becomes 'up')
        (...)
    }

I altered the Quaternion to Euler conversion code to match Wikipedia's example (though I don't fully understand it), i.e., the assignments of a12 all the way to a33, and then to calculate linear acceleration I just subtract from lin_acc, without the workaround proposed in #30. Here's the full code:

    void update_euler(float qw, float qx, float qy, float qz) {
        float a12, a22, a31, a32, a33;  // rotation matrix coefficients for Euler angles and gravity components
        a12 = 2.0f * (qw * qz + qx * qy);
        a22 = 1.0f - 2.0f * (qy * qy + qz * qz);
        a31 = 2.0f * (qw * qx + qy * qz);
        a32 = 2.0f * (qw * qy - qz * qx);
        a33 = 1.0f - 2.0f * (qx * qx + qy * qy);
        euler[0] = atan2f(a31, a33);
        euler[1] = asinf(a32);
        euler[2] = atan2f(a12, a22);
        euler[0] *= 180.0f / PI;
        euler[1] *= 180.0f / PI;
        euler[2] *= 180.0f / PI;
        euler[2] += magnetic_declination;
        if (euler[2] >= +180.f)
            euler[2] -= 360.f;
        else if (euler[2] < -180.f)
            euler[2] += 360.f;

        lin_acc[0] = a[0] - a31;
        lin_acc[1] = a[1] - a32;
        lin_acc[2] = a[2] - a33;
    }

This seems to have fixed everything. I would like to ask the contributors of this library whether this could be a solution (though these changes are for ENU and you probably want to keep NED).

[kriswiner]

If it works for you then it is a solution. The "library" is a collection of simple sketches that work for me. Everyone's situation is going to be different, so the sketches are supposed to serve as a starting point...looks like you have done the right thing by undertsanding enough about them to modify to taste.

…

On Sun, Mar 14, 2021 at 12:49 PM Tiago Lopes ***@***.***> wrote: I believe this is related to #30 <#30>. After investigating, I figured the initial Roll being around 179º (negative or positive) instead of 0 is because the library is using NED coordinates. They are interesting for Aircraft applications, as: Coordinates to describe observations made from an aircraft are normally given relative to its intrinsic axes, but normally using as positive the coordinate pointing downwards, where the interesting points are located. Therefore, they are normally NED. <https://en.wikipedia.org/wiki/Axes_conventions#Conventions_for_aircraft_local_reference_frames> In other words, Roll with NED coordinates *is supposed* to be 0 only when the IMU sensor is pointing at the ground, and 180º when pointing at the sky. Naturally, this is great for pilots figuring out whether things took a turn for the worse (😛), but in my project, ENU seems more appropriate, i.e., I want that Roll be 0 when the IMU is laying flat and pointing at the sky. Anyway, here's what I did. @kriswiner <https://github.com/kriswiner>, the author of the library this one is based on, claims switching from NED to ENU won't break the filters: The sensor data need to be fed into the Madgwick or Mahony filter in a consistent way, usually NED but could be ENU. Since to convert NED to ENU you swap X and Y axes and negate Z, I made the following change in the update() function (I ignored mn, me and md because they aren't used by the Mahony filter): bool update() { (...) quat_filter.update(ae, an, -ad, ge, gn, -gd, mn, me, md, q); //the 'd' in ad and gd thus becomes 'u' (i.e., 'down' becomes 'up') (...) } I altered the Quaternion to Euler conversion code to match Wikipedia's example <https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2> (though I don't fully understand it), i.e., the assignments of a12 all the way to a33, and then to calculate linear acceleration I just subtract from lin_acc, without the workaround proposed in #30 <#30>. Here's the full code: void update_euler(float qw, float qx, float qy, float qz) { float a12, a22, a31, a32, a33; // rotation matrix coefficients for Euler angles and gravity components a12 = 2.0f * (qw * qz + qx * qy); a22 = 1.0f - 2.0f * (qy * qy + qz * qz); a31 = 2.0f * (qw * qx + qy * qz); a32 = 2.0f * (qw * qy - qz * qx); a33 = 1.0f - 2.0f * (qx * qx + qy * qy); euler[0] = atan2f(a31, a33); euler[1] = asinf(a32); euler[2] = atan2f(a12, a22); euler[0] *= 180.0f / PI; euler[1] *= 180.0f / PI; euler[2] *= 180.0f / PI; euler[2] += magnetic_declination; if (euler[2] >= +180.f) euler[2] -= 360.f; else if (euler[2] < -180.f) euler[2] += 360.f; lin_acc[0] = a[0] - a31; lin_acc[1] = a[1] - a32; lin_acc[2] = a[2] - a33; } This seems to have fixed everything. I would like to ask the contributors of this library whether this could be a solution (though these changes are for ENU and you probably want to keep NED). — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#37 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ABTDLKVZNHY4SRA7HJLGOEDTDUHMLANCNFSM4ZEITASQ> .

[vollukas]

void update_euler(float qw, float qx, float qy, float qz) {
        float a12, a22, a31, a32, a33;  // rotation matrix coefficients for Euler angles and gravity components
        a12 = 2.0f * (qw * qz + qx * qy);
        a22 = 1.0f - 2.0f * (qy * qy + qz * qz);
        a31 = 2.0f * (qw * qx + qy * qz);
        a32 = 2.0f * (qw * qy - qz * qx);
        a33 = 1.0f - 2.0f * (qx * qx + qy * qy);
        euler[0] = atan2f(a31, a33);
        euler[1] = asinf(a32);
        euler[2] = atan2f(a12, a22);
        euler[0] *= 180.0f / PI;
        euler[1] *= 180.0f / PI;
        euler[2] *= 180.0f / PI;
        euler[2] += magnetic_declination;
        if (euler[2] >= +180.f)
            euler[2] -= 360.f;
        else if (euler[2] < -180.f)
            euler[2] += 360.f;

        lin_acc[0] = a[0] - a31;
        lin_acc[1] = a[1] - a32;
        lin_acc[2] = a[2] - a33;
    }

Thanks, OldSpice7! :) I couldn't figure out what was wrong when it seemed like it's working, but Roll and Pitch were on a different axis than expected and Roll values alternated between positive and negative max as you said.

Your solution was exactly what I needed for my Quadcopter school project as I wouldn't like to mount an MPU module standing just to get the correct axis. I am glad you posted this solution just a few weeks before I needed it :D I can't believe there aren't better documentation and more tutorials for such a useful module.

I only noticed that the Pitch value is reversed as I would expect the nose to be in the direction of the Y-axis arrow which is painted on the MPU9250 module. I believe that when the nose is pointing towards the sky, it should give a positive value. My solution would be to simply negate Pitch value if the reverse value was actually a problem for my project... But I just wonder how would I need to change your code in order to flip the polarity of Pitch.

[froussel]

I am looking to make a compass that will work on a sailboat.
Concerning the value of the roll I have the same problem I would have to turn the sensor upside down to have a variation around 0
The modification mentioned above does not work.
My level in mathematics and programming does not allow me to act to get there.
Help !

[kriswiner]

The sensor data need to be fed into the fusion algorithm in NED orientation. So figure out in you application which sensor axes are pointing North, East, and Down and adjust input accordingly.

…

On Tue, Apr 6, 2021 at 1:20 PM froussel ***@***.***> wrote: I am looking to make a compass that will work on a sailboat. Concerning the value of the roll I have the same problem I would have to turn the sensor upside down to have a variation around 0 The modification mentioned above does not work. My level in mathematics and programming does not allow me to act to get there. Help ! — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#37 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ABTDLKRGWO6C52WLCQ4MPPDTHNUI5ANCNFSM4ZEITASQ> .

[froussel]

Thanks but I don't know how to do it

[kriswiner]

?? aN, aE, aD, gN, gE, gD, mN, mE, mD

…

On Tue, Apr 6, 2021 at 2:52 PM froussel ***@***.***> wrote: Thanks but I don't know how to do it — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#37 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ABTDLKU5664RDP5NNPWWCKTTHN7BFANCNFSM4ZEITASQ> .

[froussel]

I see the mpu9250.h file contains a lot of stuff but it's too complex for me
I work as an amateur, I take a piece of code that I can get) to work then I modify it to see how it works using the examples from the library.
Example:

• use setMagneticDeclination () ... Where should we call this function? After or before calibration?
• use selectFilter (): Which is the best one to make a tilt compensated digital compass?

@kriswiner was kind enough to answer me, thanks, you have to understand that I do not understand anything :-)
Do I just have to turn my sensor over so that roll is around 0 °?
After https://github.com/hideakitai/MPU9250/issues/43 is the goal I'm looking for

[kriswiner]

Sorry, I do not have a recipe for you. If you don't understand anything, spend a few hours or days reading...

…

On Wed, Apr 7, 2021 at 6:27 AM froussel ***@***.***> wrote: I see the mpu9250.h file contains a lot of stuff but it's too complex for me I work as an amateur, I take a piece of code that I can get) to work then I modify it to see how it works using the examples from the library. Example: - use setMagneticDeclination () ... Where should we call this function? After or before calibration? - use selectFilter (): Which is the best one to make a tilt compensated digital compass? @kriswiner <https://github.com/kriswiner> was kind enough to answer me, thanks, you have to understand that I do not understand anything :-) Do I just have to turn my sensor over so that roll is around 0 °? After #43 <http://url> is the goal I'm looking for — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#37 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ABTDLKSHCXZGOKH3ZSUZYALTHRMUFANCNFSM4ZEITASQ> .

[froussel]

Friendly
float an = -a[0]; //
float ae = +a[1]; //
float ad = +a[2]; //
By groping I recovered a value of roll and pitch but without understanding.

Read only in English ... It's not my mother tongue and try to see why in English: I am cognitively overloaded :-)

[hideakitai]

@froussel @vollukas This issue will be fixed by https://github.com/hideakitai/MPU9250/tree/bugfix_obromious. Please confirm if it works.

[hideakitai]

Fixed in v0.4.3. Please reopen the issue if you still have the problem.