Determination of orientation in space (yaw, pitch, roll) exclusively by using a magnetometer

Other Hardware Sensors
1

Hello, I have been having the problem of determining the yaw, pitch and roll of an airplane for some time.

I have already tried this with a gyroscope and an accelerometer, but I have not been successful. This is because the acceleration/movement of my airplane has too great an effect on the acceleration sensor and therefore on the final result. I have already tried various fusion / ahrs algorithms, but none of them have delivered 100% convincing results.

Therefore I wonder if it is possible to determine the yaw, pitch and roll of my model airplane by using a magnetometer only. Are there already algorithms that realize this? Or do I need an accelerometer for this application under all circumstances? Perhaps there is also an algorithm that works exclusively with a gyroscope and a magnetometer.

I am open to all suggestions. Thank you!

2

The magnetometer cannot be used to measure pitch and roll, which requires a "down" direction reference. To measure all three angles, you need the accelerometer for pitch and roll (relative to down) and yaw (relative to north).

For any fusion algorithm to work, the gyro and the magnetometer must be calibrated, and for best results, the accelerometer should also be calibrated. The best overview on the latter two calibration procedures (which use the same approach) is Tutorial: How to calibrate a compass (and accelerometer) with Arduino | Underwater Arduino Data Loggers

3

Thank you for your answer.

I guess I'll have to discard the idea of using the magnetometer (i.e. using it alone for the determination)

I am using your MPU6050 Mahony Fusion class from your Github account and have followed the calibration steps (for both the gyroscope and the accelerometer) that you linked in your answer. Unfortunately, acceleration still changes the angles (pitch and roll) even though the sensor is on a flat table and I just move it around.

Is the Mahony filter even able to correct or recognize this kind of movement? And can you think of any other sources of error why I have this kind of problem?

4

Please post the results of the calibration process, and the code, using code tags, and I'll take a look.

5

Gyro Offsets: {-109.73, -118.21, -24.61}
Accel Offsets: {-132.0, 293.0, -645.0}
Accel Scales: {1.005, 0.997, 1.011}

// MPU-6050 Mahony IMU  (yaw angle is relative to starting orientation)
// last update 7/9/2020

#include "Wire.h"

// AD0 low = 0x68 (default for Sparkfun module)
// AD0 high = 0x69
int MPU_addr = 0x68;

// vvvvvvvvvvvvvvvvvv  VERY VERY IMPORTANT vvvvvvvvvvvvvvvvvvvvvvvvvvvvv
//These are the previously determined offsets and scale factors for accelerometer and gyro for
// a particular example of an MPU-6050. They are not correct for other examples.
//The IMU code will NOT work well or at all if these are not correct

float A_cal[6] = {-132.0, 293.0, -645.0, 1.005, 0.997, 1.011}; // 0..2 offset xyz, 3..5 scale xyz
// the code will work, but not as accurately, for an uncalibrated accelerometer. Use this line instead:
// float A_cal[6] = {0.0, 0.0, 0.0, 1.000, 1.000, 1.000}; // 0..2 offset xyz, 3..5 scale xyz

float G_off[3] = {-109.73, -118.21, -24.61}; //raw offsets, determined for gyro at rest
#define gscale ((250./32768.0)*(PI/180.0))  //gyro default 250 LSB per d/s -> rad/s

// ^^^^^^^^^^^^^^^^^^^ VERY IMPORTANT ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

// GLOBALLY DECLARED, required for Mahony filter
// vector to hold quaternion
float q[4] = {1.0, 0.0, 0.0, 0.0};

// Free parameters in the Mahony filter and fusion scheme,
// Kp for proportional feedback, Ki for integral
float Kp = 30.0;
float Ki = 0.0;

// with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.
// with MPU-6050, some instability observed at Kp=100 Now set to 30.

// char s[60]; //snprintf buffer, if needed

// globals for AHRS loop timing
unsigned long now_ms, last_ms = 0; //millis() timers

// print interval
unsigned long print_ms = 200; //print angles every "print_ms" milliseconds
float yaw, pitch, roll; //Euler angle output

void setup() {

  Wire.begin();
  Serial.begin(9600);
  Serial.println("starting");

  // initialize sensor
  // defaults for gyro and accel sensitivity are 250 dps and +/- 2 g
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

}

//--------------------------------------------------------------------------------------------------
// Mahony scheme uses proportional and integral filtering on
// the error between estimated reference vector (gravity) and measured one.
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date      Author      Notes
// 29/09/2011 SOH Madgwick    Initial release
// 02/10/2011 SOH Madgwick  Optimised for reduced CPU load
// last update 07/09/2020 SJR minor edits
//--------------------------------------------------------------------------------------------------
// IMU algorithm update

void Mahony_update(float ax, float ay, float az, float gx, float gy, float gz, float deltat) {
  float recipNorm;
  float vx, vy, vz;
  float ex, ey, ez;  //error terms
  float qa, qb, qc;
  static float ix = 0.0, iy = 0.0, iz = 0.0;  //integral feedback terms
  float tmp;

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  tmp = ax * ax + ay * ay + az * az;
  if (tmp > 0.0)
  {

    // Normalise accelerometer (assumed to measure the direction of gravity in body frame)
    recipNorm = 1.0 / sqrt(tmp);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Estimated direction of gravity in the body frame (factor of two divided out)
    vx = q[1] * q[3] - q[0] * q[2];  //to normalize these terms, multiply each by 2.0
    vy = q[0] * q[1] + q[2] * q[3];
    vz = q[0] * q[0] - 0.5f + q[3] * q[3];

    // Error is cross product between estimated and measured direction of gravity in body frame
    // (half the actual magnitude)
    ex = (ay * vz - az * vy);
    ey = (az * vx - ax * vz);
    ez = (ax * vy - ay * vx);

    // Compute and apply to gyro term the integral feedback, if enabled
    if (Ki > 0.0f) {
      ix += Ki * ex * deltat;  // integral error scaled by Ki
      iy += Ki * ey * deltat;
      iz += Ki * ez * deltat;
      gx += ix;  // apply integral feedback
      gy += iy;
      gz += iz;
    }

    // Apply proportional feedback to gyro term
    gx += Kp * ex;
    gy += Kp * ey;
    gz += Kp * ez;
  }

  // Integrate rate of change of quaternion, q cross gyro term
  deltat = 0.5 * deltat;
  gx *= deltat;   // pre-multiply common factors
  gy *= deltat;
  gz *= deltat;
  qa = q[0];
  qb = q[1];
  qc = q[2];
  q[0] += (-qb * gx - qc * gy - q[3] * gz);
  q[1] += (qa * gx + qc * gz - q[3] * gy);
  q[2] += (qa * gy - qb * gz + q[3] * gx);
  q[3] += (qa * gz + qb * gy - qc * gx);

  // renormalise quaternion
  recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
  q[0] = q[0] * recipNorm;
  q[1] = q[1] * recipNorm;
  q[2] = q[2] * recipNorm;
  q[3] = q[3] * recipNorm;
}

// AHRS loop

void loop()
{
  static float deltat = 0;  //loop time in seconds
  static unsigned long now = 0, last = 0; //micros() timers

  //raw data
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  int16_t Tmp; //temperature

  //scaled data as vector
  float Axyz[3];
  float Gxyz[3];


  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14); // request a total of 14 registers
  int t = Wire.read() << 8;
  ax = t | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  t = Wire.read() << 8;
  ay = t | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  t = Wire.read() << 8;
  az = t | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  t = Wire.read() << 8;
  Tmp = t | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  t = Wire.read() << 8;
  gx = t | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  t = Wire.read() << 8;
  gy = t | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  t = Wire.read() << 8;
  gz = t | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  Axyz[0] = (float) ax;
  Axyz[1] = (float) ay;
  Axyz[2] = (float) az;

  //apply offsets and scale factors from Magneto
  for (int i = 0; i < 3; i++) Axyz[i] = (Axyz[i] - A_cal[i]) * A_cal[i + 3];

  Gxyz[0] = ((float) gx - G_off[0]) * gscale; //250 LSB(d/s) default to radians/s
  Gxyz[1] = ((float) gy - G_off[1]) * gscale;
  Gxyz[2] = ((float) gz - G_off[2]) * gscale;

  //  snprintf(s,sizeof(s),"mpu raw %d,%d,%d,%d,%d,%d",ax,ay,az,gx,gy,gz);
  //  Serial.println(s);

  now = micros();
  deltat = (now - last) * 1.0e-6; //seconds since last update
  last = now;

  Mahony_update(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], deltat);

  // Compute Tait-Bryan angles. Strictly valid only for approximately level movement

  // In this coordinate system, the positive z-axis is up, X north, Y west.
  // Yaw is the angle between Sensor x-axis and Earth magnetic North
  // (or true North if corrected for local declination, looking down on the sensor
  // positive yaw is counterclockwise, which is not conventional for NED navigation.
  // Pitch is angle between sensor x-axis and Earth ground plane, toward the
  // Earth is positive, up toward the sky is negative. Roll is angle between
  // sensor y-axis and Earth ground plane, y-axis up is positive roll. These
  // arise from the definition of the homogeneous rotation matrix constructed
  // from quaternions. Tait-Bryan angles as well as Euler angles are
  // non-commutative; that is, the get the correct orientation the rotations
  // must be applied in the correct order which for this configuration is yaw,
  // pitch, and then roll.
  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
  // which has additional links.

  // WARNING: This angular conversion is for DEMONSTRATION PURPOSES ONLY. It WILL
  // MALFUNCTION for certain combinations of angles! See https://en.wikipedia.org/wiki/Gimbal_lock

  roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
  pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
  //conventional yaw increases clockwise from North. Not that the MPU-6050 knows where North is.
  yaw   = -atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3]));
  // to degrees
  yaw   *= 180.0 / PI;
  if (yaw < 0) yaw += 360.0; //compass circle
  pitch *= 180.0 / PI;
  roll *= 180.0 / PI;

  now_ms = millis(); //time to print?
  if (now_ms - last_ms >= print_ms) {
    last_ms = now_ms;
    // print angles for serial plotter...
    //  Serial.print("ypr ");
    Serial.print(yaw, 0);
    Serial.print(", ");
    Serial.print(pitch, 0);
    Serial.print(", ");
    Serial.println(roll, 0);
  }
}
6

Nothing obviously wrong with the code.

I didn't read this comment as closely as I should have.

Pitch and roll are defined relative to "down", which is the direction measured for the acceleration due to gravity. Any additional acceleration violates that assumption, and leads to incorrect values of pitch and roll. The gyro helps to correct those errors, but only if the sensor is rotating.

7

Ah, I actually thought that, but I wasn't sure.

I didn't know that rotation was necessary for the correction. Very interesting!

But unfortunately I don't think that solves my problem. For example, at the phase where the airplane takes off, the acceleration is large and the change in angle is small. Are there other algorithms that can reduce this acceleration influence or is my situation hopeless? Because the angle change is surprisingly large with this type of movement (I have already seen up to 25° with only a small acceleration)

What surprises me is that there are conventional flight controllers that seem to cope perfectly well with a gyroscope and accelerometer :thinking:

8

That is the definition of "sensor fusion". The integrated rates of rotation, leading to an estimate of the 3D orientation, helps in the process of defining "down".

One approach to mitigating the effects of external applied acceleration is to discount orientation estimates, if the magnitude of the measured acceleration vector is much different than expected for gravity alone.

Keep in mind that the MPU-6050 was discontinued many years ago, so what you have is some sort of clone or counterfeit. A modern 9DOF IMU from a trusted source will perform much better.

9

Okay, that doesn't sound very good. It basically means that I can hardly rely on the angle calculation during the flight.

Could you imagine that my problems come from the MPU6050 and another sensor (newer gyroscope and accelerometer) does not have these problems? Or is this a general problem?

And could you kindly name an alternative sensor whose quality you are more convinced of? That would be a great help.

10

9DOF IMUs work very well for approximately level flight at roughly constant speed, which is most of flying. If you want to make something that does controlled acrobatics, that will require custom tuned, extremely sophisticated code and high quality sensors.

11

No, that's definitely not my plan. I'm really only interested in flying straight ahead and making a few turns. I just thought that accelerating (speeding up the plane) or turning would cause massive problems, but apparently 9DOF IMUs can handle it.

What about that?

12

9DOF sensors from trusted suppliers (Adafruit, Pololu, Sparkfun, Conrad, etc.) will work. The market is competitive and as a result the sensors all perform similarly.

13

Thank you very much.

I will order the new sensor in the next few days and test it with your Mahony algorithm. I hope I have more success with it.

I'll get back to you with my results in a few days. Have a nice evening and thank you for your help!

14

Hey, I ordered the ICM-20948 a few days ago and it works much better than the MPU-6050. I did a short test flight yesterday and the data seems to be much more suitable. Thanks for your help @jremington

15

The ICM-20948 is a good choice, and I'm glad to hear that you are happy with it.

16

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