AZ3166 sensors orientation in Madgwick

[luc08]

Hi, it's my first project with an Arduino board. I'm trying to use the sensors on the AZ3166 board to calculate pitch, roll and yaw. I'm taking some samples at rest to calculate the accelerometer and gyroscope (LSM6DSL) offset, then I used magneto software to calibrate the magnetometer(LIS2MDL). I'm using the Madgwick ARHS library and I don't know how to implement the magnetometer in the Madgwick's equation. I converted the mdps to dps, mg to g and miligauss to µT. From the datasheet, LSM6DSL sensors have a right-handed orientation and LIS2MDL is left-handed, how to match the coordinate system? when i'm only using the acc and gyro the heading decreases by 0.1 degree every 10 seconds.

filter.update(gx,gy,gz,ax,ay,az,mx,my,mz);

lis2mdl.pdf (1.09 MB)

[dyso]

https://forum.arduino.cc/index.php?topic=714950.0

Take a look at this. You have to consider which direction is a positive pitch and positive roll. Then think about the gyro and accelerometer separately.

Also, the MadgwickAHRS library already converts deg into rads. So your gyro needs to be in deg.

[luc08]

Thank you for your reply, it's really appreciated but It's my first time and I'm a bit lost. What would you suggest me if I want a North-East-Down orientation using my sensors ? I tried to change (my) to (-my) but the heading takes several seconds to stabilize when I moved the board.

filter.update(gx,gy,gz,ax,ay,az,mx,-my,mz);

[dyso]

The thing is, a Madgwick filter is just fusing the gyroscope with the magnetometer and fusing the gyroscope with the accelerometer.

If you look at the turn on the y axis, it rotates downwards, but a positive pitch is reversed. If you look at your z axis, it rotates anticlockwise, but a compass rotates clockwise.

So, for your pitch and roll it has to look like this (my assumption is that your magnetometer has to look like this aswell):

filter.update(gx,-gy,-gz,-ax,ay,az,-mx,my,mz);

I would advise you to read up on a complimentary filter to get a concept of fusion because complimentary filter is very simple.

Edit: I see, the magnetometer looks different. However, the gyro and accelerometer is the same. Therefore, try:

 -mx, -my, mz

[dyso]

To imagine it better, look at the picture.

I still don't understand why I had to reverse mx. I have compared the Madgwick library with the paper from Madgwick and it all looks correct. I also checked his calculations in the paper and tried them all on my own.

[luc08]

Thank you again Dyso for your support, I tried the solutions that you proposed to me and it is still not working. The heading time response is really slow and when I'm moving the board, it takes sometimes to calculate the heading and stabilize the value. Could it be a problem with the gyro time integration? (acc/gyro alone work fine)

[dyso]

It is not advisable to fuse the gyro with the magnetometer. I was told this by jremington and I also did different tests. Therefore, for the heading, you could program a tilt-compensated heading indicator that gets compensated by the roll and pitch from the Madgwick filter.

Could it be a problem with the gyro time integration? (acc/gyro alone work fine)

Regarding the gyro, it could be. Maybe you should upload your code.

[jremington]

Hi, luc08

Hi, it's my first project with an Arduino board.

This is a very challenging "first project"! To understand 9DOF sensor data fusing, I strongly recommend this discussion.

As mentioned in reply #1, this call won't work with your sensor, because the axes of the sensors are not aligned consistently.

filter.update(gx,gy,gz,ax,ay,az,mx,my,mz);

The Madgwick/Mahony code assumes that the first sensor axis in the function call points to magnetic North, but the illustration in the sensor data sheet assumes that the accelerometer Y axis and the gyro Y (R) axis point toward magnetic North, while the magnetometer Y axis is assumed to point magnetic South.

Furthermore, the magnetometer axes are defined in a left handed coordinate system, which must be corrected.

If you want to stick with this data sheet convention, then I believe that the filter call should be as follows, and you have to hold the sensor module as illustrated, with accel Z up and Y North. Unfortunately, I don't have one of those sensors to test it.

filter.update(gy,gx,-gz,ay,ax,-az,-my,mx,-mz);

Notes: The Euler angles that you derive from the quaternion produced by the filter won't necessarily have the correct sign, because (1) there is no universal convention on what constitutes a positive rotation (and the mathematical definition of a positive rotation angle used in trigonometry does not agree with conventional navigation) and (2) there are around a dozen ways to define Euler angles. So feel free to change the sign of yaw, pitch or roll to suit yourself.

Finally, this is not necessary, as the filter uses normalized accelerometer and magnetometer readings (vector magnitude =1.0)

I converted ... mg to g and miligauss to µT.

[dyso]

But with that you turned the IMU 90 deg clockwise, right? Usually the x-axis should be aligned with the heading. I find rotating the IMU or completely changing the naming to be confusing (Pitch to roll).

Tbh, I agree with @jremington that this is very challenging for the first project. Therefore, I would strongly advise you to read up on the complimentary filter to get a basic knowledge of Fusion.

[jremington]

Usually the x-axis should be aligned with the heading

The naming of the sensor axes is completely arbitrary.

The illustration in the LSM6DSL data sheet assumes that the accel and gyro Y axes are pointing to magnetic North.

The Madgwick and Mahony filters assume that the sensor X axes are pointing to magnetic North, so you either have to orient the sensor accordingly, or swap axes in the code. And since the magnetometer axes are defined inconsistently, and in a left handed system, you have to swap those anyway or the filter won't work.

To have the accel/gyro X axis point at magnetic North for yaw=0, while holding the sensor so that Z points down, I think the call would be

filter.update(gx, gy, gz, ax, ay, az, mx, -my, mz);

[dyso]

Here, copy this and complete this code tailored to your IMU:

// Include IMU Library
#include <MadgwickAHRS.h>  // Click here to get the library: https://github.com/arduino-libraries/MadgwickAHRS
#include <MahonyAHRS.h>    // Click here to get the library: https://github.com/PaulStoffregen/MahonyAHRS

int period = 100;
unsigned long time_now = 0;

//Mahony filter;
Madgwick filter;

int SR = 1000/period;

void setup() {
Serial.begin(9600);
// Needs more set-up from your IMU 

filter.begin(SR); // Sampling rate in Hz
}

void loop()
{
float ax, ay, az, gx, gy, gz, mx, my, mz;

if(millis() >= time_now + period){
   time_now += period;

// Get directly from IMU
ax = ;
ay = ;
az = ;

gx = ;
gy = ;
gz = ;

mx = ;
my = ;
mz = ;

/*
Serial.print(millis()/1000.00);
Serial.print(", ");

Serial.print(ax);
Serial.print(", ");
Serial.print(ay);
Serial.print(", ");
Serial.println(az);

Serial.print(gx);
Serial.print(", ");
Serial.print(gy);
Serial.print(", ");
Serial.println(gz);

Serial.print(mx);
Serial.print(", ");
Serial.print(my);
Serial.print(", ");
Serial.println(mz);
*/

// Update the filter
    
//filter.updateIMU(gx, -gy, -gz, -ax, ay, az);
filter.update(gx, -gy, -gz, -ax, ay, az, -mx, -my, mz);

// Print the orientation filter output
float roll = filter.getRoll();
float pitch = filter.getPitch();
float heading = filter.getYaw();
Serial.print(millis());
Serial.print(" - Orientation: ");
Serial.print(heading);
Serial.print(" ");
Serial.print(pitch);
Serial.print(" ");
Serial.print(roll);

Serial.println();

}
}

Also, I would advise you to first test

filter.updateIMU()

See if your gyroscope does a full 360° roll. First start with 90 deg, then 180 then 360. If it works then it is ready to be fused with the mag. If rotation is anti clockwise, then check your gz. also, make sure that if you pitch up, that the pitch changes positive and that the angle is correct. You can use a protractor. If it is correct, then roll the IMU towards the right, if it is positive then it is correct, also use a protractor here. Make sure that these values do not oscillate strongly. They tend to do this if you haven't switched the axis correctly or if the sampling rate does not agree with the filter.

[dyso]

jremington:
The illustration in the LSM6DSL data sheet assumes that the accel and gyro Y axes are pointing to magnetic North.

You are right! I didn't see that. But it does depend on how its mounted on the Breakout board.