Question on Yaw Behavior

[mjs513]

I have been using the Madgwick and Mahoney filters for years and recently have went back to my old stuff to do some more work and found that you updated the filter. So far have done some preliminary testing using the Teensy Propshield from PJRC and a MPU9250 (amazon variety). So far its been working rather well. But with that said I ran into something interesting with the MPU9250 with yaw behavior with the MPU9250: (1) I am seeing yaw going in the opposite direction of movement for a short period of time before coming catching up to the direction of movement and (2) takes a really long time to reach final position after movement.

This is best shown in the following series of charts:

1. Steady State before rotating sensor:

2. Rotation started around 7.5 seconds as can be seen in both the gyro chart and the Euler angles (note the chart in the lower left are a plot of all the flags which remain at 0). In the figure you can see the yaw movement delayed and moving up and then down.

3. Steady state reached in its new position: Seems to take over 20 seconds to get to where its going.

Not sure what is happening.

I am feeding the algorithm precalibrated values accel and mag. Gyro;s are zeroed out before starting the algorithm so again precalibrated. Calibration takes into account only biases and scale factors. I am running at 100hz.

Also I am using the default settings:

    const FusionAhrsSettings settings = {
            .convention = FusionConventionEnu,
            .gain = 0.5f,
            .accelerationRejection = 10.0f,
            .magneticRejection = 20.0f,
            .rejectionTimeout = 5.0f * SAMPLE_RATE, /* 5 seconds */
    };

Any suggestions would be appreciated.

Here is the whole sketch if you want to see whats going on:

#include "MPU9250.h"
#include "Fusion.h"
#include <stdbool.h>
#include <stdio.h>

#include <Wire.h>
#include "EEPROM.h"
#include "Streaming.h"
#include <string>

#define cout Serial

elapsedMillis dump;

float SAMPLE_RATE; // replace this with actual sample RATE in Hz
float g = 9.80665;
float rads2degs = 57.296;

FusionOffset offset;
FusionAhrs ahrs;
FusionQuaternion q;
FusionEuler euler;
FusionAhrsFlags flags;

float val[10];

uint64_t timestamp;
uint8_t raw_values_on = 0;
uint8_t x_values_on = 0;
uint8_t serial_values_on = 0;
uint8_t telem_values_on = 0;

// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;

void setup() {
  // serial to display data
  Serial.begin(57200);
  SerialUSB1.begin(57200);
  while(!Serial && millis() < 2000) {}

  Wire.begin(400000);
  // start communication with IMU 
  status = IMU.begin();
  if (status < 0) {
    SerialUSB1.println("IMU initialization unsuccessful");
    SerialUSB1.println("Check IMU wiring or try cycling power");
    SerialUSB1.print("Status: ");
    SerialUSB1.println(status);
    while(1) {}
  }

  cout.println("IMU Connected!");

  //Load calibration from EEPROM
  loadCal();

  // setting the accelerometer full scale range to +/-8G 
  IMU.setAccelRange(MPU9250::ACCEL_RANGE_2G);
  // setting the gyroscope full scale range to +/-500 deg/s
  IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
  // setting DLPF bandwidth to 20 Hz
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  // setting SRD to 19 for a 50 Hz update rate
  // rate = 1000/(SRD + 1) HZ
  //IMU.setSrd(19);
  //SAMPLE_RATE = 0.02f;
  //SRD for 100 hz = 9
  IMU.setSrd(9);
  SAMPLE_RATE = 0.01f;
  // enabling the data ready interrupt
  IMU.enableDataReadyInterrupt();
  // attaching the interrupt to microcontroller pin 1
  pinMode(1,INPUT);
  attachInterrupt(1,getIMU,RISING);
  //set intial Gyro Calibration
  IMU.calibrateGyro();
  SerialUSB1.println("Gyro Cal completed");

    FusionOffsetInitialise(&offset, SAMPLE_RATE * 100);
    FusionAhrsInitialise(&ahrs);

    // Set AHRS algorithm settings
    const FusionAhrsSettings settings = {
            .convention = FusionConventionEnu,
            .gain = 0.5f,
            .accelerationRejection = 10.0f,
            .magneticRejection = 20.0f,
            .rejectionTimeout = 5.0f * SAMPLE_RATE, /* 5 seconds */
    };
    FusionAhrsSetSettings(&ahrs, &settings);

  menu();

  IMU.calibrateGyro();
  SerialUSB1.println("Gyro Cal completed");
}

void loop() {
    if ( SerialUSB1.available() ) {
      char rr;
      rr = SerialUSB1.read();
      switch (rr) {
        case 'c':
          rr ='0';
          print_CAL();
          break;
        case 'r':
        {
          rr ='0';
          if(raw_values_on == 1) {
            raw_values_on = 0;
          } else if(raw_values_on == 0) {
            raw_values_on = 1;
          }
        }
          break;
        case 's':
        {
          rr ='0';
          if(serial_values_on == 1) {
            serial_values_on = 0;
          } else if(serial_values_on == 0) {
            serial_values_on = 1;
          }
        }
          break;
        case 'x':
        {
          rr ='0';
          if(x_values_on == 1) {
            x_values_on = 0;
          } else if(serial_values_on == 0) {
            x_values_on = 1;
          }
        }
            break;
        case 't':
        {
          rr ='0';
          if(telem_values_on == 1) {
            telem_values_on = 0;
          } else if(telem_values_on == 0) {
            telem_values_on = 1;
          }
        }
            break;
        case 'g': IMU.calibrateGyro();Serial.println("Gyro Cal completed");break;
        case '\r':
        case '\n':
        case 'h': menu(); break;
      }
      while (Serial.read() != -1) ; // remove rest of characters.
    }

}

void getIMU() {
  // read the sensor
  IMU.readSensor(val);

  // Acquire latest sensor data
  FusionVector gyroscope = {val[3]*rads2degs, val[4]*rads2degs, val[5]*rads2degs}; // replace this with actual gyroscope data in degrees/s
  FusionVector accelerometer = {val[0]/g, val[1]/g, val[2]/g}; // replace this with actual accelerometer data in g
  FusionVector magnetometer = {val[6], val[7], -val[8]}; // replace this with actual magnetometer data in arbitrary units

  flags = FusionAhrsGetFlags(&ahrs);

  gyroscope = FusionOffsetUpdate(&offset, gyroscope);
  // Calculate delta time (in seconds) to account for gyroscope sample clock error
  FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, SAMPLE_RATE);
  euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
  q = FusionAhrsGetQuaternion(&ahrs);

  //if(dump > 100) {
    if(raw_values_on == 1) {
      print_float_array(val, 10);
      Serial.println();
    }

    if(serial_values_on == 1) {
      Serial.printf("Fusion (RPY): %0.1f, %0.1f, %0.1f\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
    }

    if(x_values_on == 1) {
      timestamp = micros();
      char accelgyroBuffer[100];
      sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f,%0.6f,%0.6f,%0.6f", 'I', timestamp, val[3]*rads2degs, val[4]*rads2degs, val[5]*rads2degs, val[0]/g, val[1]/g, val[2]/g);
      Serial.printf ("%s\n",accelgyroBuffer);
      sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f", 'M', timestamp, val[6], val[7], -val[8]);
      Serial.printf ("%s\n",accelgyroBuffer);
      sprintf(accelgyroBuffer, "%c,%llu,%0.6f,%0.6f,%0.6f,%0.6f", 'Q', timestamp, q.element.w, q.element.x, q.element.y, q.element.z);
      Serial.printf ("%s\n",accelgyroBuffer);
    }

    if(telem_values_on == 1) telemetryPortOut();
    dump = 0;
  //}
}

void loadCal() {
  // EEPROM buffer and variables to load accel and mag bias
  // and scale factors from CalibrateMPU9250.ino
  uint8_t EepromBuffer[48];
  float a_xb, a_xs, a_yb, a_ys, a_zb, a_zs;
  float hxb, hxs, hyb, hys, hzb, hzs;

 // load and set accel and mag bias and scale
  // factors from CalibrateMPU9250.ino
  for (size_t i = 0; i < sizeof(EepromBuffer); i++) {
    EepromBuffer[i] = EEPROM.read(i);
  }
  memcpy(&a_xb, EepromBuffer + 0, 4);
  memcpy(&a_xs, EepromBuffer + 4, 4);
  memcpy(&a_yb, EepromBuffer + 8, 4);
  memcpy(&a_ys, EepromBuffer + 12, 4);
  memcpy(&a_zb, EepromBuffer + 16, 4);
  memcpy(&a_zs, EepromBuffer + 20, 4);
  memcpy(&hxb, EepromBuffer + 24, 4);
  memcpy(&hxs, EepromBuffer + 28, 4);
  memcpy(&hyb, EepromBuffer + 32, 4);
  memcpy(&hys, EepromBuffer + 36, 4);
  memcpy(&hzb, EepromBuffer + 40, 4);
  memcpy(&hzs, EepromBuffer + 44, 4);

  IMU.setAccelCalX(a_xb, a_xs);
  IMU.setAccelCalY(a_yb, a_ys);
  IMU.setAccelCalZ(a_zb, a_zs);

  IMU.setMagCalX(hxb, hxs);
  IMU.setMagCalY(hyb, hys);
  IMU.setMagCalZ(hzb, hzs);
}

void print_float_array(float *arr, int size) {
    if (size == 0) {
        Serial.printf("[]");
    } else {     
        Serial.print('[');
        for (int i = 0; i < size - 1; i++)
            Serial.printf("%f, ", arr[i]);
        Serial.printf("%f]", arr[size - 1]);
    }
}

void menu()
{
  SerialUSB1.println();
  SerialUSB1.println("Menu Options:");
  SerialUSB1.println("\tx - x-IMU3 GUI Output");
  SerialUSB1.println("\tt - Telemetry Viewer Output");
  SerialUSB1.println("\ts - Serial Print Output (Euler Angles)");
  SerialUSB1.println("\tc - Print Calibration");
  SerialUSB1.println("\tr - Print Raw Values");
  SerialUSB1.println("\tg - Zero the Gyros");
  SerialUSB1.println("\th - Menu");
  SerialUSB1.println();
}

void print_CAL() {
  // EEPROM buffer and variables to load accel and mag bias
  // and scale factors from CalibrateMPU9250.ino
  uint8_t EepromBuffer[48];
  float a_xb, a_xs, a_yb, a_ys, a_zb, a_zs;
  float hxb, hxs, hyb, hys, hzb, hzs;

 // load and set accel and mag bias and scale
  // factors from CalibrateMPU9250.ino
  for (size_t i = 0; i < sizeof(EepromBuffer); i++) {
    EepromBuffer[i] = EEPROM.read(i);
  }
  memcpy(&a_xb, EepromBuffer + 0, 4);
  memcpy(&a_xs, EepromBuffer + 4, 4);
  memcpy(&a_yb, EepromBuffer + 8, 4);
  memcpy(&a_ys, EepromBuffer + 12, 4);
  memcpy(&a_zb, EepromBuffer + 16, 4);
  memcpy(&a_zs, EepromBuffer + 20, 4);
  memcpy(&hxb, EepromBuffer + 24, 4);
  memcpy(&hxs, EepromBuffer + 28, 4);
  memcpy(&hyb, EepromBuffer + 32, 4);
  memcpy(&hys, EepromBuffer + 36, 4);
  memcpy(&hzb, EepromBuffer + 40, 4);
  memcpy(&hzs, EepromBuffer + 44, 4);

  Serial.println("Accelerometer bias and scale factors:");
  cout.print( a_xb, 6); cout.print(", "); Serial.println(a_xs, 6);
  cout.print( a_yb, 6); cout.print(", "); Serial.println(a_ys, 6);
  cout.print( a_zb, 6); cout.print(", "); Serial.println(a_zs, 6);
  Serial.println(" ");
  Serial.println("Magnetometer bias and scale factors:");
  cout.print( hxb, 6); cout.print(", "); Serial.println(hxs, 6);
  cout.print( hyb, 6); cout.print(", "); Serial.println(hys, 6);
  cout.print( hzb, 6); cout.print(", "); Serial.println(hzs, 6);
  Serial.println(" ");

}

void telemetryPortOut(){  
  // Set textLength to the number of parameters to print * 31
  int  textLength = 21 * 31;
  char text[textLength];

  //Temporary text parameters
  char gyroxText[30];
  char gyroyText[30];
  char gyrozText[30];

  char accxText[30];
  char accyText[30];
  char acczText[30];

  char magxText[30];
  char magyText[30];
  char magzText[30];

  char pitchText[30];
  char rollText[30];
  char yawText[30];

  char qwText[30];
  char qxText[30];
  char qyText[30];
  char qzText[30];

  char f01[30]; char f02[30]; char f03[30]; char f04[30]; char f05[30];

  dtostrf(val[3]*rads2degs, 10, 10, gyroxText);
  dtostrf(val[4]*rads2degs, 10, 10, gyroyText);
  dtostrf(val[5]*rads2degs, 10, 10, gyrozText);
  dtostrf(val[0]/g, 10, 10, accxText);
  dtostrf(val[1]/g, 10, 10, accyText);
  dtostrf(val[2]/g, 10, 10, acczText);
  dtostrf(val[6], 10, 10, magxText);
  dtostrf(val[7], 10, 10, magyText);
  dtostrf(-val[8], 10, 10, magzText);
  dtostrf(euler.angle.pitch, 10, 10, pitchText);
  dtostrf(euler.angle.roll, 10, 10, rollText);
  dtostrf(euler.angle.yaw, 10, 10, yawText);

  dtostrf( q.element.w, 10, 6, qwText);
  dtostrf(q.element.x, 10, 6, qxText);
  dtostrf(q.element.y, 10, 6, qyText);
  dtostrf(q.element.z, 10, 6, qzText);  

  dtostrf((float)flags.initialising, 10, 6, f01);
  dtostrf((float)flags.accelerationRejectionWarning, 10, 6, f02);
  dtostrf((float)flags.accelerationRejectionTimeout, 10, 6, f03);
  dtostrf((float)flags.magneticRejectionWarning, 10, 6, f04);
  dtostrf((float)flags.magneticRejectionTimeout, 10, 6, f05);

  // Create single text parameter and print it
  snprintf(text, textLength, "%s, %s, %s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s",
           gyroxText, gyroyText, gyrozText,
           accxText, accyText, acczText,
           magxText, magyText, magzText,
           pitchText, rollText, yawText,
           qwText, qxText, qyText, qzText,
           f01, f02, f03, f04, f05);
    Serial.println(text);  
  //Serial.send_now();

}    

[xioTechnologies]

I expect your magnetometer needs calibrating. I suggest you use FusionAhrsUpdateNoMagnetometer instead of FusionAhrsUpdate to see the behaviour without the magnetometer. You may also wish to plot the output of FusionCompassCalculateHeading for known rotations to see exactly what heading (yaw) measurement your magnetometer is providing.

[mjs513]

Thanks for the fast response and will give those suggestions a try and see how it goes. Have to set up a rotation rose on the desk :) Project for tomorrow. Getting late here now. Will let you know how it goes

[mjs513]

I expect your magnetometer needs calibrating. I suggest you use FusionAhrsUpdateNoMagnetometer instead of FusionAhrsUpdate to see the behaviour without the magnetometer. You may also wish to plot the output of FusionCompassCalculateHeading for known rotations to see exactly what heading (yaw) measurement your magnetometer is providing.

Took your advice and went back to looking at calibration methods. Found I had one issue with the way I was doing the calibration which I fixed and also switched to using NED coordinates to match what the how the library I am using has the axes aligned.

That seemed to fix a couple of issues I was having (yes I am using a compass rose as well to check yaw and heading). However with that said I am still seing a lag in yaw as shown in the next too figures using the default settings:

    const FusionAhrsSettings settings = {
            .convention = FusionConventionEnu,
            .gain = 0.5f,
            .accelerationRejection = 10.0f,
            .magneticRejection = 20.0f,
            .rejectionTimeout = 5.0f * SAMPLE_RATE, /* 5 seconds */
    };

from the figures you can see it took about 12 seconds for yaw to converge.

Now if I change the settings to:

    const FusionAhrsSettings settings = {
            .convention = FusionConventionNed,
            .gain = 1.5f,
            .accelerationRejection = 10.0f,
            .magneticRejection = 5.0f,
            .rejectionTimeout = 5.0f * SAMPLE_RATE, /* 5 seconds */
    };

its much improved:

Am I on the right track here. And is there any other guidance on adjusting the settings that you can provide other than what is in the READ.ME?

[xioTechnologies]

Your plots are of little use because you have not provided any indication what the actual rotation is. You need to compare your measured rotation with the actual rotation. For example, rotate the device 90° and compare this with the measured rotation.

[mjs513]

Your plots are of little use because you have not provided any indication what the actual rotation is. You need to compare your measured rotation with the actual rotation. For example, rotate the device 90° and compare this with the measured rotation.

Note: working on my desk and now looking at it like this still looks off a bit I think. Well back to calibration.

Sorry thought I mentioned that, hope this is what you are talking about

[xioTechnologies]

Good luck with calibration. I will now close this issue.