Yaw angle slowly spins when using MPU9250

Hello, everyone.

First of all, thank you for visiting my post.

I'm currently working on a project that involves using the MPU9250 sensor to measure object orientation.

However, measured yaw angle gradually decreases over time, even when the sensor is stationary.

I would be thankful if I someone can help me troubleshoot this problem.

Here, are some details about my setup:

My code is as following.

#include <MadgwickAHRS.h>

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU9250.h"

MPU9250 accelgyro;
I2Cdev I2C_M;
Madgwick filter;

uint8_t buffer_m[6];

int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;

float heading;
float tilt_heading;
float A_xyz[3];
float G_xyz[3];
float M_xyz[3];
#define sample_num_mdate 5000
volatile float mx_sample[3];
volatile float my_sample[3];
volatile float mz_sample[3];

static float mx_centre = 0;
static float my_centre = 0;
static float mz_centre = 0;

volatile int mx_max = 0;
volatile int my_max = 0;
volatile int mz_max = 0;

volatile int mx_min = 0;
volatile int my_min = 0;
volatile int mz_min = 0;

float temperature;
float pressure;
float atm;
float altitude;

int sample_rate;
unsigned long micros_per_reading, micros_previous;
float roll, pitch, yaw;

void setup() {
  // put your setup code here, to run once:
  Wire.begin();
  Serial.begin(38400);
  accelgyro.initialize();
  
  sample_rate = 50;
  filter.begin(sample_rate);
  micros_per_reading = 1000000 / sample_rate;
  micros_previous = micros();
  delay(1000);
}

void loop() {
  // put your main code here, to run repeatedly:
  get_accel_data();
  get_gyro_data();
  get_compass_date_calibrated();
  get_heading();
  get_tilt_heading();

  unsigned long micros_now;
  // check if it's time to read data and update the filter
  micros_now = micros();
  if (micros_now - micros_previous >= micros_per_reading) {
    // update the filter, which computes orientation
    filter.updateIMU(G_xyz[0], G_xyz[1], G_xyz[2], A_xyz[0], A_xyz[1], A_xyz[2]);

    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    Serial.print("Orientation: ");
    Serial.print(heading);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);

    // increment previous time, so we keep proper pace
    micros_previous = micros_previous + micros_per_reading;
  }

  Serial.flush();
  Serial.print(A_xyz[0]);
  Serial.print(",");
  Serial.print(A_xyz[1]);
  Serial.print(",");
  Serial.print(A_xyz[2]);
  Serial.print(",");
  Serial.print(G_xyz[0]);
  Serial.print(",");
  Serial.print(G_xyz[1]);
  Serial.print(",");
  Serial.print(G_xyz[2]);
  Serial.print(",");
  Serial.print(M_xyz[0]);
  Serial.print(",");
  Serial.print(M_xyz[1]);
  Serial.print(",");
  Serial.print(M_xyz[2]);
  Serial.print(",");
  Serial.print(heading);
  Serial.print(",");
  Serial.print(tilt_heading);
  Serial.println();
}

void get_heading(void)
{
  heading = 180 * atan2(M_xyz[1], M_xyz[0]) / PI;
  if (heading < 0) heading += 360;
}

void get_tilt_heading(void)
{
  float pitch = asin(-A_xyz[0]);
  float roll = asin(A_xyz[1] / cos(pitch));

  float xh = M_xyz[0] * cos(pitch) + M_xyz[2] * sin(pitch);
  float yh = M_xyz[0] * sin(roll) * sin(pitch) + M_xyz[1] * cos(roll) - M_xyz[2] * sin(roll) * cos(pitch);
  float zh = -M_xyz[0] * cos(roll) * sin(pitch) + M_xyz[1] * sin(roll) + M_xyz[2] * cos(roll) * cos(pitch);
  tilt_heading = 180 * atan2(yh, xh) / PI;
  if (yh < 0)    tilt_heading += 360;
}

void Mxyz_init_calibrated ()
{

  Serial.println(F("Before using 9DOF,we need to calibrate the compass frist,It will takes about 2 minutes."));
  Serial.print("  ");
  Serial.println(F("During  calibratting ,you should rotate and turn the 9DOF all the time within 2 minutes."));
  Serial.print("  ");
  Serial.println(F("If you are ready ,please sent a command data 'ready' to start sample and calibrate."));
  while (!Serial.find("ready"));
  Serial.println("  ");
  Serial.println("ready");
  Serial.println("Sample starting......");
  Serial.println("waiting ......");

  get_calibration_Data ();

  Serial.println("     ");
  Serial.println("compass calibration parameter ");
  Serial.print(mx_centre);
  Serial.print("     ");
  Serial.print(my_centre);
  Serial.print("     ");
  Serial.println(mz_centre);
  Serial.println("    ");
}


void get_calibration_Data ()
{
  for (int i = 0; i < sample_num_mdate; i++)
  {
      get_one_sample_date_mxyz();

      if (mx_sample[2] >= mx_sample[1])mx_sample[1] = mx_sample[2];
      if (my_sample[2] >= my_sample[1])my_sample[1] = my_sample[2]; //find max value
      if (mz_sample[2] >= mz_sample[1])mz_sample[1] = mz_sample[2];

      if (mx_sample[2] <= mx_sample[0])mx_sample[0] = mx_sample[2];
      if (my_sample[2] <= my_sample[0])my_sample[0] = my_sample[2]; //find min value
      if (mz_sample[2] <= mz_sample[0])mz_sample[0] = mz_sample[2];

  }

  mx_max = mx_sample[1];
  my_max = my_sample[1];
  mz_max = mz_sample[1];

  mx_min = mx_sample[0];
  my_min = my_sample[0];
  mz_min = mz_sample[0];

  mx_centre = (mx_max + mx_min) / 2;
  my_centre = (my_max + my_min) / 2;
  mz_centre = (mz_max + mz_min) / 2;
}

void get_one_sample_date_mxyz()
{
  get_compass_data();
  mx_sample[2] = M_xyz[0];
  my_sample[2] = M_xyz[1];
  mz_sample[2] = M_xyz[2];
}

void get_accel_data(void)
{
  accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
  A_xyz[0] = (double) ax / 16384;
  A_xyz[1] = (double) ay / 16384;
  A_xyz[2] = (double) az / 16384;
}

void get_gyro_data(void)
{
  accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);

  G_xyz[0] = (double) gx * 250 / 32768;
  G_xyz[1] = (double) gy * 250 / 32768;
  G_xyz[2] = (double) gz * 250 / 32768;
}

void get_compass_data(void)
{
  I2C_M.writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer
  delay(10);
  I2C_M.readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer_m);

  mx = ((int16_t)(buffer_m[1]) << 8) | buffer_m[0] ;
  my = ((int16_t)(buffer_m[3]) << 8) | buffer_m[2] ;
  mz = ((int16_t)(buffer_m[5]) << 8) | buffer_m[4] ;

  M_xyz[0] = (double) mx * 1200 / 4096;
  M_xyz[1] = (double) my * 1200 / 4096;
  M_xyz[2] = (double) mz * 1200 / 4096;
}

void get_compass_date_calibrated ()
{
  get_compass_data();
  M_xyz[0] = M_xyz[0] - mx_centre;
  M_xyz[1] = M_xyz[1] - my_centre;
  M_xyz[2] = M_xyz[2] - mz_centre;
}

Serial ouput's format is as following.:
Orientation: [Yaw] [Pitch] [Roll]

acc_x, acc_y, acc_z, g_x, g_y, g_z, mag_x, mag_y, mag_z, heading, tilt_heading

And serial output is as following:

Orientation: 179.95 0.49 0.87

-0.04,0.04,1.00,6.71,-0.11,-1.01,14.06,-13.77,-22.56,179.95,315.68

Orientation: 179.93 0.66 1.17

-0.03,0.04,0.99,6.96,-0.14,-0.81,13.48,-14.65,-23.44,179.93,312.81

Orientation: 179.92 0.85 1.43

-0.03,0.03,0.99,7.01,-0.30,-0.80,13.77,-13.48,-23.73,179.92,315.69

Orientation: 179.91 1.03 1.71

-0.03,0.04,0.99,7.00,-0.24,-0.72,13.77,-14.06,-23.14,179.91,314.70

Orientation: 179.89 1.25 1.88

-0.03,0.03,0.99,6.84,-0.02,-1.00,14.06,-14.36,-22.56,179.89,314.34

Orientation: 179.87 1.47 2.09

-0.03,0.04,0.99,6.97,-0.11,-0.81,13.77,-13.18,-23.44,179.87,316.55

Orientation: 179.86 1.69 2.26

-0.03,0.04,0.99,6.87,-0.14,-0.86,14.06,-13.77,-23.73,179.86,315.91

Orientation: 179.84 1.86 2.25

-0.03,0.03,0.98,6.65,-0.17,-0.83,13.77,-14.36,-22.85,179.84,313.83

Orientation: 179.81 1.85 2.15

-0.03,0.03,0.99,6.87,-0.42,-0.83,13.77,-14.36,-23.73,179.81,313.88

Orientation: 179.79 1.62 2.27

-0.03,0.04,0.99,7.16,-0.31,-1.06,14.65,-14.36,-22.85,179.79,316.00

Orientation: 179.77 1.79 2.27

-0.03,0.03,0.99,7.12,-0.33,-0.94,14.06,-13.48,-23.14,179.77,316.34

Orientation: 179.74 1.97 2.27

-0.04,0.04,0.99,7.06,-0.12,-0.98,13.77,-14.06,-23.14,179.74,314.37

Orientation: 179.72 1.83 2.23

-0.03,0.03,0.99,6.97,-0.08,-0.78,14.06,-14.36,-23.14,179.72,314.60

Orientation: 179.70 1.84 2.13

-0.03,0.04,0.99,6.78,-0.11,-0.85,14.06,-14.36,-22.85,179.70,314.64

Orientation: 179.68 2.06 2.33

-0.03,0.04,0.99,6.68,0.02,-1.00,13.48,-14.65,-23.14,179.68,312.66

Orientation: 179.66 2.14 2.26

-0.04,0.04,0.99,6.95,-0.01,-0.74,13.77,-14.94,-22.85,179.66,312.56

Orientation: 179.63 1.99 2.22

-0.03,0.04,0.99,6.68,-0.19,-0.82,14.06,-13.77,-23.44,179.63,315.76

Orientation: 179.61 1.87 2.16

-0.03,0.04,0.99,6.93,-0.13,-0.91,13.77,-14.06,-23.14,179.61,314.55

Orientation: 179.59 2.09 2.25

-0.04,0.04,0.99,6.97,-0.14,-0.88,13.48,-14.06,-23.44,179.59,313.69

Orientation: 179.57 1.97 2.18

-0.03,0.04,0.99,6.72,-0.18,-0.88,13.48,-14.36,-22.85,179.57,313.18

Orientation: 179.56 2.07 2.53

-0.03,0.04,0.99,6.81,0.08,-0.86,14.06,-14.06,-22.85,179.56,315.26

Orientation: 179.53 2.07 2.44

-0.04,0.04,0.99,6.94,-0.37,-0.90,14.06,-14.36,-22.56,179.53,314.41

Orientation: 179.51 1.98 2.36

-0.03,0.03,0.99,6.83,0.11,-0.70,13.77,-14.65,-22.85,179.51,313.23

Orientation: 179.48 1.84 2.32

-0.03,0.04,0.99,6.83,-0.23,-0.84,13.18,-14.36,-23.14,179.48,312.74

Orientation: 179.46 1.92 2.24

-0.03,0.03,0.99,6.80,-0.40,-0.74,13.77,-14.06,-22.85,179.46,314.30

Orientation: 179.44 2.12 2.26

-0.04,0.04,0.99,6.86,-0.10,-1.09,12.89,-13.77,-23.73,179.44,312.84

Orientation: 179.41 2.17 2.18

-0.04,0.04,0.99,6.87,-0.27,-0.85,14.06,-14.06,-23.14,179.41,315.02

Orientation: 179.39 2.02 2.15

-0.03,0.03,0.98,7.11,-0.12,-0.85,13.77,-13.77,-22.56,179.39,315.01

Orientation: 179.37 1.85 2.13

-0.03,0.03,0.99,6.89,-0.19,-0.69,13.77,-14.06,-23.14,179.37,314.52

Orientation: 179.34 1.95 2.06

-0.03,0.04,0.99,6.77,-0.21,-1.17,14.65,-14.65,-23.14,179.34,315.17

Orientation: 179.31 2.02 1.98

-0.03,0.03,0.99,6.86,0.02,-0.82,13.77,-13.77,-23.14,179.31,314.93

Orientation: 179.29 1.84 1.97

-0.03,0.03,0.99,6.68,-0.14,-0.85,13.77,-14.06,-22.85,179.29,314.37

Orientation: 179.28 2.03 2.23

-0.03,0.04,0.99,6.63,-0.24,-0.85,14.06,-14.06,-23.14,179.28,315.10

Orientation: 179.25 2.05 2.13

-0.04,0.04,0.99,6.62,-0.06,-0.85,13.48,-13.77,-23.44,179.25,314.42

Orientation: 179.09 1.76 2.23

-0.03,0.04,0.99,6.73,-0.12,-0.92,13.18,-14.65,-22.85,179.09,312.19

Orientation: 179.06 1.86 2.15

-0.03,0.04,0.99,6.58,0.11,-0.91,14.06,-14.36,-22.85,179.06,314.63

Orientation: 179.04 1.86 2.07

-0.03,0.03,0.99,6.99,-0.10,-0.84,13.48,-13.77,-23.44,179.04,314.34

Orientation: 179.02 1.65 2.10

-0.03,0.03,0.99,7.08,-0.31,-0.81,13.48,-14.06,-23.14,179.02,314.02

Orientation: 179.00 1.82 2.09

-0.03,0.03,0.98,6.95,-0.25,-0.92,13.77,-14.06,-23.73,179.00,314.28

Orientation: 178.98 2.02 2.33

-0.03,0.04,0.99,6.85,-0.21,-1.05,13.77,-13.77,-23.73,178.98,315.24

Orientation: 178.95 2.11 2.27

-0.04,0.04,0.99,7.05,-0.11,-1.13,13.48,-14.06,-23.14,178.95,313.70

Orientation: 178.93 1.88 2.38

-0.03,0.04,0.99,7.03,-0.20,-0.92,14.06,-14.06,-23.14,178.93,315.39

Orientation: 178.90 1.88 2.29

-0.03,0.03,0.99,7.14,-0.12,-0.91,13.77,-14.65,-23.14,178.90,313.06

Orientation: 178.88 1.80 2.21

-0.03,0.04,0.99,6.78,-0.27,-0.95,13.77,-14.36,-22.85,178.88,314.02

Orientation: 178.86 1.69 2.15

-0.03,0.04,0.99,6.98,-0.29,-0.78,13.18,-14.06,-23.44,178.86,313.38

Orientation: 178.84 1.87 2.42

-0.03,0.04,0.99,7.07,-0.34,-0.80,13.48,-14.36,-22.85,178.84,313.44

Orientation: 178.82 1.81 2.34

-0.03,0.04,0.99,7.00,-0.25,-0.92,13.77,-14.06,-22.56,178.82,314.72

Orientation: 178.79 1.95 2.29

-0.03,0.04,0.99,6.83,-0.02,-0.92,14.06,-14.36,-23.14,178.79,314.47

Orientation: 178.77 1.89 2.21

-0.03,0.03,0.99,6.89,0.06,-1.03,13.77,-14.36,-23.44,178.77,313.84

Orientation: 178.74 1.92 2.12

-0.03,0.03,0.99,6.99,-0.37,-0.85,14.36,-14.06,-22.85,178.74,315.61

Orientation: 178.71 1.91 2.03

-0.03,0.03,0.99,7.01,0.07,-0.97,13.48,-14.06,-23.73,178.71,313.78

Orientation: 178.69 1.90 1.94

-0.03,0.03,0.99,6.90,-0.14,-1.02,13.48,-14.65,-22.85,178.69,312.55

Thank you for reading.

Two possible problems:

  1. Magnetometers must be very carefully calibrated. Your procedure calculates crude offsets, but does not scale the axes to be in agreement. For a much more accurate approach, see Tutorial: How to calibrate a compass (and accelerometer) with Arduino | Underwater Arduino Data Loggers

  2. The axial orientations of the magnetometer and the accelerometer/gyro must agree and form right handed coordinate systems. The default labels on the sensor axes do not agree, so the magnetometer axes need to be swapped and/or sign changes made appropriately.

From the data sheet:

Thank you for your answer!

Right now I'm kinda scraping available sources on internet to measure attitude of object.

Is there any proper process to achieve this task with MPU9250?

Also, it seems like my magnetometer reading is spinning as shown in the serial output ([heading] value is decreasing).

[heading] value is calculated as following:

heading = 180 * atan2(M_xyz[1], M_xyz[0]) / PI

Another problem is that the gyroscope offset is not nice and constant, it drifts over time. You or the AHRS library must constantly update the offset correction or you get this "spinning" when the board is stationary.

Check the readme of this library for some more details. Also nice to check their implementation of the offset-correction algorithm in there (found in FusionOffset.c).
https://github.com/xioTechnologies/Fusion

This one works, but each individual sensor must be properly calibrated. An example for a difficult case is given here: https://forum.pololu.com/t/correcting-the-balboa-magnetometer/14315

Temperature changes are almost all of that problem, and since the sensor measures the temperature, it is possible to correct for the offset-dependence on temperature (yet another calibration step).

Thank you for providing nice reference!

Thank you very much!

One question, when running MPU9250_cal.ino, how do I construct correction matrix and offset vector?

Sorry for keep bothering you. You are really helping me right now...

Run the program magneto. See the example linked at the end of reply #6.

For some reason i though this was handled automatically in the IMU-chip, but that seems to be the case only on the high-end products.

Oh well, more calibration code then :slight_smile:

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.