Mahoney Filter Drift

Breakout boards, sensors, other Adafruit kits, etc.

Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.
Locked
User avatar
Autonomous_Sailboat
 
Posts: 31
Joined: Thu Dec 22, 2022 10:26 pm

Mahoney Filter Drift

Post by Autonomous_Sailboat »

I am trying to get accurate and reliable Heading data from the Adafruit LSM6DSOX+LIS3MDL 9DOF sensor (https://www.adafruit.com/product/4517). Yes, I have perused through the Forum comments and see that several others have brought up the issue of magnetometer data drift before me. I think I have discovered that the drift error is sourced to the Mahoney filter code instead of to the sensor.

I ran the Examples->lis3mdl_demo sketch on my Arduino Uno R3 and drew magnetometer data from the LSM6DSOX+LIS3MDL 9DOF sensor. I then used the CoolTerm software tool to capture and store the sensor's data while it was absolutely unmoved during the 700 second test. I put that data into a spreadsheet chart and the graphic illustrates the raw data's stability in the X-, Y-, and Z-directions.
Static LIS3MDL Raw Data Capture
Static LIS3MDL Raw Data Capture
Static_LIS3MDL_Raw_Data_Capture-20221228.jpg (442.16 KiB) Viewed 168 times
I then used the Examples->Adafruit_AHRS->calibration sketch and the MotionCal software tool to capture the sensor's calibration data and stored that data in my Arduino Uno's EEPROM memory.

I then used the Examples->Adafruit_AHRS->calibrated_orientation sketch to display the sensor's filtered Heading data. While watching the data stream I noticed that the Heading data would 'drift' so I again used the CoolTerm software tool to capture the data reflecting the Mahoney filter's output for over 1,100 seconds in duration. The spreadsheet chart below illustrates how the sensor's filtered Heading data drifts even though the sensor is completely immobile. Specifically, while the Pitch and Roll data seems to drift around a baseline approximating a sine wave, the Heading data continuously increments linearly around the compass rose.
Static LIS3MDL Mahoney Filtered Data Capture
Static LIS3MDL Mahoney Filtered Data Capture
Static_LIS3MDL_Normalized_Data_Capture-20221228.jpg (445.05 KiB) Viewed 168 times
My conclusion is this: The LSM6DSOX+LIS3MDL's magnetometer sensor was demonstrated to provide a stable data stream. That stable data provided to the Mahoney filter routine in the Examples->Adafruit_AHRS->calibrated_orientation sketch, however, was demonstrated to continuously inject an error giving the appearance of sensor 'drift.'

As you would expect, I have two questions in my quest to get accurate and reliable Heading data:
  • What have I not done that I should do?
  • What have I done that I should not have done?

User avatar
sj_remington
 
Posts: 998
Joined: Mon Jul 27, 2020 4:51 pm

Re: Mahoney Filter Drift

Post by sj_remington »

The Mahony filter integrates gyro, magnetometer and accelerometer data. Gyro drift, which is as bad or worse than magnetometer drift, might be a factor.

If you suspect a problem with the code itself, then ignore the sensor data and inject constant (but reasonable) values for the 9 data items, and test again.

User avatar
sj_remington
 
Posts: 998
Joined: Mon Jul 27, 2020 4:51 pm

Re: Mahoney Filter Drift

Post by sj_remington »

I should have added that if you are serious about getting the most accurate orientation data out of the filter, you need to very carefully calibrate, and confirm the calibration, of all three sensors: gyro, magnetometer and accelerometer. How have you done that?

Incidentally, for the static test suggested in the previous post, "reasonable values" for the rate gyro components would either be all zeros, or small, Gaussian distributed values with zero mean.

User avatar
Autonomous_Sailboat
 
Posts: 31
Joined: Thu Dec 22, 2022 10:26 pm

Re: Mahoney Filter Drift

Post by Autonomous_Sailboat »

sj_remington:

I greatly appreciate the very quick response.

You asked how I performed the calibration for all three sensors (gyro, magnetometer, and accelerometer). I used the Examples->Adafruit_AHRS->calibration.ino sketch to obtain the data from the LSM6DSOX+LIS3MDL 9DOF sensor and then used the MotionCal software tool to both perform the sensor calibration routine and to upload the calibration data to the Arduino Uno's EEPROM. I believe I followed the posted calibration instructions correctly.

How serious am I about getting the most accurate orientation data out of the filter? Well, I would like to obtain "usable" data but I am not expecting surveyor-grade accuracy/precision. I would say that sensor heading data within +/-3 degrees of the actual bearing would be extremely satisfactory (that is on par with an experienced human using a handheld compass) but could probably 'live' with +/-10 degrees for the kind of navigation I intend to perform.

What concerns me after looking at the two graphics provided earlier is that it **appears** to me that the filter calculations appear to be 'chained'. What I mean by this is that one Heading calculation provides slightly erroneous output data that is used in a subsequent calculation... and that data error is compounded when used in the subsequent calculation... and so on. I have absolutely no idea if this is how the Mahoney filter algorithm is implemented... but that would account for the linear 'drift' seen in the second graphic - **especially** since the raw sensor data is static (see the first graphic provided earlier).

If that is the case then I could easily see how the Arduino Uno's floating point limitations would inject a residual error **every** time a calculation is performed and thus the Heading 'drift' in the second graphic provided above. I have already stumbled upon the Uno's floating point calculation limitations when using the spherical coordinate system equations for global navigation. The roundoff errors render the Uno's use for such calculations out of the question (and explains why I have migrated to the Arduino Due as mentioned in a separate and recent post).

So my question back to you is this: If I introduced calibration errors in any of the sensor components (gyro, magnetometer, and accelerometer) because of a poorly performed calibration procedure, would that cause a **cumulative** error (i.e., a linear 'drift') in a static sensor's filtered data as depicted in the second graphic (vice a random noise error that would 'hover' around an averaged value)?

Because if not then I am left scratching my balding head wondering what to do next. Use a different sensor? Not use a filter and thus ignore Pitch and Roll influences on Heading readings?

User avatar
sj_remington
 
Posts: 998
Joined: Mon Jul 27, 2020 4:51 pm

Re: Mahoney Filter Drift

Post by sj_remington »

The Mahony filter maintains an internal quaternion that represents the orientation, and is updated every time the filter is called with the current sensor inputs. If there is a time-dependent drift on any of the sensor readings, the quaternion and the orientation angles calculated from it will drift.

I take it you have not yet tried the suggestion I made for testing the code, ignoring the sensor input.

There is an easy way to check if you have calibrated all the sensors and applied the results correctly: output a set of corrected sensor data and run that through the calibration procedure again. The recalculated offsets should all be about zero, and for the accelerometer/magnetometer the correction matrix should be about 1 on the diagonal and 0 elsewhere.

The best overview and tutorial on calibrating magnetometers and accelerometers is this one: https://thecavepearlproject.org/2015/05 ... r-arduino/

User avatar
Autonomous_Sailboat
 
Posts: 31
Joined: Thu Dec 22, 2022 10:26 pm

Re: Mahoney Filter Drift

Post by Autonomous_Sailboat »

sj_remington:

OK, I was able to carve out some time to get to your suggestion, specifically:
If you suspect a problem with the code itself, then ignore the sensor data and inject constant (but reasonable) values for the 9 data items, and test again.
This evening I did just that but the results did more to uncover a separate issue than the one I was trying to address. What I did was use the Examples->Adafruit_AHRS->calibration sketch to capture the raw LSM6DSOX+LIS3MDL 9DOF sensor output data. I used the output from one single data collection instance and hardcoded those 9 values into the Examples->Adafruit_AHRS->calibrated_orientation sketch. Doing so would represent an immobile sensor so that I could determine if there really is a code-caused 'drift.'

I then rotated the sensor and repeated this procedure in order to capture a second set of data and hardcoded that into the sketch. Doing so enables the illustration of the Mahoney filter's response to a heavily controlled sensor orientation change. See the code where I highlighted my changes.

Code: Select all

// This sketch is based upon the Arduino Examples-> Adafruit AHRS-> calibrated_orientation sketch.
// The purpose of this modification is to capture data to explore the effects of Mahoney filter calculations.
// Two instances of simulated 9DOF sensor data are injected into the data stream in order to illustrate the output.
// The output data was formatted as a .csv in order to utilize spreadsheet graphical tools.

// This sketch injects 'hardcoded' static LSM6DSOX and LIS3MDL sensor data.
// Doing so will reveal whether observed Heading 'drift' is due to the sensor or calculations.
// Sensor data was collected using the 
//
// Full orientation sensing using NXP/Madgwick/Mahony and a range of 9-DoF
// sensor sets.
// You *must* perform a magnetic calibration before this code will work.
//
// To view this data, use the Arduino Serial Monitor to watch the
// scrolling angles, or run the OrientationVisualiser example in Processing.
// Based on  https://github.com/PaulStoffregen/NXPMotionSense with adjustments
// to Adafruit Unified Sensor interface

#include <Adafruit_Sensor_Calibration.h>
#include <Adafruit_AHRS.h>

Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;

// uncomment one combo 9-DoF!
#include "LSM6DS_LIS3MDL.h"  // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
//#include "LSM9DS.h"           // LSM9DS1 or LSM9DS0
//#include "NXP_FXOS_FXAS.h"  // NXP 9-DoF breakout

// pick your filter! slower == better quality output
//Adafruit_NXPSensorFusion filter; // slowest
//Adafruit_Madgwick filter;  // faster than NXP
Adafruit_Mahony filter;  // fastest/smalleset

#if defined(ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM)
  Adafruit_Sensor_Calibration_EEPROM cal;
#else
  Adafruit_Sensor_Calibration_SDFat cal;
#endif

#define FILTER_UPDATE_RATE_HZ 100
#define PRINT_EVERY_N_UPDATES 10
//#define AHRS_DEBUG_OUTPUT

uint32_t timestamp;

unsigned long StartTime;  // Added this to record elapsed time
float ElapsedTime;        // Added this to record elapsed time

void setup() {
  Serial.begin(115200);
  while (!Serial) yield();

  if (!cal.begin()) {
    Serial.println("Failed to initialize calibration helper");
  } else if (! cal.loadCalibration()) {
    Serial.println("No calibration loaded/found");
  }




  /*******************************************************************************************************************/
  //  MEMORANDUM:
  //  The Examples->Adafruit Sensor Calibration->sensor_calibration_read sketch was used to capture 
  //  the calibration data stored in the Arduino Uno's EEPROM
      /*
        Calibration filesys test
        Has EEPROM: 1
        Has FLASH: 0
        ------------
        0x75, 0x54, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB8, 0x1E, 0x24, 0xC2, 0xB8, 0x1E,
        0xF3, 0x41, 0x9A, 0x99, 0xED, 0xC1, 0xF6, 0x28, 0x35, 0x42, 0x23, 0xDB, 0x79, 0x3F, 0x60, 0xE5,
        0x80, 0x3F, 0x98, 0x6E, 0x82, 0x3F, 0x0A, 0xD7, 0x23, 0x3D, 0x6F, 0x12, 0x03, 0x3C, 0xA6, 0x9B,
        0x44, 0xBC, 0xE3, 0x07,
        ------------
        Calibrations found:
          Magnetic Hard Offset: -41.03, 30.39, -29.70
          Magnetic Soft Offset: 0.98, 0.04, 0.01, 0.04, 1.01, -0.01, 0.01, -0.01, 1.02
          Magnetic Field Magnitude: 45.29
          Gyro Zero Rate Offset: 0.00, 0.00, 0.00
          Accel Zero G Offset: 0.00, 0.00, 0.00
       */
  /*******************************************************************************************************************/




  if (!init_sensors()) {
    Serial.println("Failed to find sensors");
    while (1) delay(10);
  }
  
  accelerometer->printSensorDetails();
  gyroscope->printSensorDetails();
  magnetometer->printSensorDetails();

  setup_sensors();
  filter.begin(FILTER_UPDATE_RATE_HZ);
  timestamp = millis();

  Wire.setClock(400000); // 400KHz

  StartTime = millis();   // Added this in order to produce time-stamped output.
}



  int PrintCounter = 0; // Added this to print heading only once
  void loop() {
  float roll, pitch, heading;
  float gx, gy, gz;
  static uint8_t counter = 0;

  if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) {
    return;
  }
  timestamp = millis();
  // Read the motion sensors
  sensors_event_t accel, gyro, mag;
  accelerometer->getEvent(&accel);
  gyroscope->getEvent(&gyro);
  magnetometer->getEvent(&mag);



  /*******************************************************************************************************************/
  //  START OF DEBUGGING MODIFICATIONS:
  //  OBSERVATION: This sketch using the Mahoney filter produces Heading data that 'drifts' linearly
  //  HYPOTHESIS: Filter calculations introduce errors due to the Arduino Uno's floating point limitations
  //  TEST: Replace received sensor data with static values, conduct filtering calculations, and observe Heading output
  //  EXPECTATION: Filtering calculations using static input data should similarly produce static Heading output data
  //  INPUT DATA: The Examples->Adafruit AHRS->calibration sketch was used to capture the following data:
    /*
        Adafruit AHRS - IMU Calibration!
        Calibration filesys test
        Loaded existing calibration
        ------------------------------------
        Sensor:       LSM6DS_A
        Type:         Acceleration (m/s2)
        Driver Ver:   1
        Unique ID:    1745
        Min Value:    -156.91
        Max Value:    156.91
        Resolution:   0.06
        ------------------------------------
        
        ------------------------------------
        Sensor:       LSM6DS_G
        Type:         Gyroscopic (rad/s)
        Driver Ver:   1
        Unique ID:    1746
        Min Value:    -34.91
        Max Value:    34.91
        Resolution:   0.00
        ------------------------------------
        
        ------------------------------------
        Sensor:       LIS3MDL
        Type:         Magnetic (uT)
        Driver Ver:   1
        Unique ID:    0
        Min Value:    -1600.00
        Max Value:    1600.00
        Resolution:   0.01
        ------------------------------------
        
        Raw:-579,1057,8177,10,-2,-13,-310,191,-678
        Uni:-0.69,1.27,9.78,0.0115,-0.0027,-0.0150,-31.04,19.12,-67.83
    */
  //  The following replaces the received sensor data with defined data
  //  For the first 600 seconds, use the data obtained where X faces North
  //  For the second 600 seconds, use the data obtained where X faces West
  //  Terminate at 1200 seconds (20 minutes)

  ElapsedTime = (float(millis()) - float(StartTime)) / 1000.0;
  if(ElapsedTime <= 600.0)
  {
    // Data has been captured in one orientation.
    accel.acceleration.x = -0.69;
    accel.acceleration.y =  1.27;
    accel.acceleration.z =  9.78;
    gx =  0.0115;
    gy = -0.0027;
    gz = -0.0150;
    mag.magnetic.x = -31.04;
    mag.magnetic.y =  19.12;
    mag.magnetic.z = -67.83;
  }
  else
  {
    if(ElapsedTime <= 1200.00)
    {
      // Data has been captured from a completely different orientation.
      accel.acceleration.x = 0.22;
      accel.acceleration.y = 0.26;
      accel.acceleration.z = 9.87;
      gx =  0.0012;
      gy = -0.0017;
      gz = -0.0044;
      mag.magnetic.x =  -78.43;
      mag.magnetic.y =   19.56;
      mag.magnetic.z = -110.63;
    }
    else
    {
      Serial.println("\n\n\nProgram execution complete.\n\n");
      while(1)
      {
        ;
      }
    }
  }

  //  END OF DEBUGGING MODIFICATIONS
  /*******************************************************************************************************************/




#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif

  cal.calibrate(mag);
  cal.calibrate(accel);
  cal.calibrate(gyro);
  // Gyroscope needs to be converted from Rad/s to Degree/s
  // the rest are not unit-important
  gx = gyro.gyro.x * SENSORS_RADS_TO_DPS;
  gy = gyro.gyro.y * SENSORS_RADS_TO_DPS;
  gz = gyro.gyro.z * SENSORS_RADS_TO_DPS;

  // Update the SensorFusion filter
  filter.update(gx, gy, gz, 
                accel.acceleration.x, accel.acceleration.y, accel.acceleration.z, 
                mag.magnetic.x, mag.magnetic.y, mag.magnetic.z);
#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif

  // only print the calculated output once in a while
  if (counter++ <= PRINT_EVERY_N_UPDATES) {
    return;
  }
  // reset the counter
  counter = 0;

#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("Raw: ");
  Serial.print(accel.acceleration.x, 4); Serial.print(", ");
  Serial.print(accel.acceleration.y, 4); Serial.print(", ");
  Serial.print(accel.acceleration.z, 4); Serial.print(", ");
  Serial.print(gx, 4); Serial.print(", ");
  Serial.print(gy, 4); Serial.print(", ");
  Serial.print(gz, 4); Serial.print(", ");
  Serial.print(mag.magnetic.x, 4); Serial.print(", ");
  Serial.print(mag.magnetic.y, 4); Serial.print(", ");
  Serial.print(mag.magnetic.z, 4); Serial.println("");
#endif


  /*******************************************************************************************************************/
  // Add the following in order to print CSV header data
  PrintCounter = PrintCounter + 1;
  if(PrintCounter == 1)
  {
    Serial.println("Calibrated orientation output\n");
    Serial.println("Time, Orientation, , , , Quaternion");
    Serial.println("(sec), Heading, Pitch, Roll, , Qw, Qx, Qy, Qz");
  }
  // Add the following in order to record the elapsed time
  ElapsedTime = (float(millis()) - float(StartTime)) / 1000.0;
  Serial.print(ElapsedTime);
  Serial.print(", ");
  /*******************************************************************************************************************/


  // print the heading, pitch and roll
  roll = filter.getRoll();
  pitch = filter.getPitch();
  heading = filter.getYaw();


  //  Below provides slightly modified output more suited to CSV data presentation
  //  Serial.print("Orientation: ");
  Serial.print(heading);
  Serial.print(", ");
  Serial.print(pitch);
  Serial.print(", ");
  Serial.print(roll);
  Serial.print(", ,");

  float qw, qx, qy, qz;
  filter.getQuaternion(&qw, &qx, &qy, &qz);
  //  Serial.print("Quaternion: ");
  Serial.print(qw, 4);
  Serial.print(", ");
  Serial.print(qx, 4);
  Serial.print(", ");
  Serial.print(qy, 4);
  Serial.print(", ");
  Serial.println(qz, 4);  
  
#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("Took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif
}
The output was then collected into .csv format, put into a spreadsheet, and I was able to display the response.

Mahoney Filter Header Calculation Response.jpg
Mahoney Filter Header Calculation Response.jpg (382.43 KiB) Viewed 123 times

While my intent was to further explore the issue of filter output data 'drift' I was more surprised with the time required to respond to any position change. Requiring three minutes for the Mahoney filter to perform the calculations in response to a large Heading change is completely unsatisfactory no matter how accurate the result may be. Yes, there is a warning embedded within the libraries indicating a performance issue... but I didn't think the response would be quite so lethargic.

Armed with this understanding I believe it is time that I take your other suggestion and dive into the thecavepearlproject.org's material to learn more. I am also going to repeat today's test on a Mega and a Due in order to compare the Mahoney filter's calculation performance. There may be some control system variables (proportion and integral gains?) that may be covered in your recommended reading that enables performance enhancement. I may even get some answers about the perceived 'drift' problem.

Tomorrow is New Year's Eve. I hope you have a Happy New Year. And Thank You for the help.

User avatar
sj_remington
 
Posts: 998
Joined: Mon Jul 27, 2020 4:51 pm

Re: Mahoney Filter Drift

Post by sj_remington »

It does look like the proportional gain is far too small Most people set the integral gain to zero.

Here is one of the very few decent discussions of the Mahony algorithm: https://nitinjsanket.github.io/tutorial ... est/mahony

By the way, there is quite a bit of AHRS code on the web that is just plain wrong. Examples include cases where people have taken code that works for one sensor, and modified it for another, but failed to correct for different axis alignments or coordinate system handedness.

User avatar
Autonomous_Sailboat
 
Posts: 31
Joined: Thu Dec 22, 2022 10:26 pm

Re: Mahoney Filter Drift

Post by Autonomous_Sailboat »

sj_remington:

Well, thanks to you I have had success in using an Arduino Due, an Adafruit LSM6DSOX_LIS3MDL 9DOF sensor, and the Mahoney filter to obtain timely and highly accurate Heading data. Yes, there is a rather short sensor warm-up period but after that the sensor is unbelievably stable with an accuracy of less than +/-2 degrees. Since a well-trained person using a handheld compass can successfully take an azimuth with an accuracy of +/-3 degrees I am VERY satisfied with this outcome. Here are the results of my initial test:

Heading Drift Test using Adruino Due.jpg
Heading Drift Test using Adruino Due.jpg (638.96 KiB) Viewed 109 times

First off, I performed a calibration re-check using the MotionCal software tool. Maybe these new values are better than what I had used in my previous post on this subject and that is why I have mitigated the Heading 'drift' problem. I dunno and at this point I don't care. Here is the calibration that enabled success:

MotionCal_Calibrated_Sensor_Data-20221231.png
MotionCal_Calibrated_Sensor_Data-20221231.png (162.45 KiB) Viewed 109 times

Since the Arduino Due does not have EEPROM memory I had to edit some code to suit my project's specific configuration. I extensively edited the Examples->Adafruit_Sensor_Calibration->Adafruit_Sensor_Calibration_EEPROM.cpp file to hardcode the sensor calibration values obtained using the MotionCal software tool into the Mahoney filter calculations and to then disable the code that attempts to read or write to the non-existent EEPROM. Here is the code for that modified file:

Code: Select all

// This file was drawn from the Examples->Adafruit_Sensor_Calibration->Adafruit_Sensor_Calibration_EEPROM.cpp
// file provided with the Arduino IDE.  It was then extensively modified to hardcode sensor calibration data
// obtained from the MotionCal software tool that reflects successful LSM6DSOX_LIS3MDL sensor calibration.
// Actions were disabled which attempt to read or write to the EEPROM.

#include "Adafruit_Sensor_Calibration.h"

#if defined(ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM)

/**************************************************************************/
/*!
    @brief Initializes Flash and filesystem
    @returns False if any failure to initialize flash or filesys
*/
/**************************************************************************/
bool Adafruit_Sensor_Calibration_EEPROM::begin(uint8_t eeprom_addr) {
  Serial.println(F("Begin"));
/*
  ee_addr = eeprom_addr;

#if defined(ESP8266) || defined(ESP32)
  EEPROM.begin(512);
#endif
*/
  return true;
}

bool Adafruit_Sensor_Calibration_EEPROM::saveCalibration(void) {
  Serial.println(F("Save Cal"));

/*
  uint8_t buf[EEPROM_CAL_SIZE];
  memset(buf, 0, EEPROM_CAL_SIZE);
  buf[0] = 0x75;
  buf[1] = 0x54;

  float offsets[16];
  memcpy(offsets, accel_zerog, 12);       // 3 x 4-byte floats
  memcpy(offsets + 3, gyro_zerorate, 12); // 3 x 4-byte floats
  memcpy(offsets + 6, mag_hardiron, 12);  // 3 x 4-byte floats

  offsets[9] = mag_field;

  offsets[10] = mag_softiron[0];
  offsets[11] = mag_softiron[4];
  offsets[12] = mag_softiron[8];
  offsets[13] = mag_softiron[1];
  offsets[14] = mag_softiron[2];
  offsets[15] = mag_softiron[5];

  memcpy(buf + 2, offsets, 16 * 4);

  uint16_t crc = 0xFFFF;
  for (uint16_t i = 0; i < EEPROM_CAL_SIZE - 2; i++) {
    crc = crc16_update(crc, buf[i]);
  }
  Serial.print("CRC: ");
  Serial.println(crc, HEX);
  buf[EEPROM_CAL_SIZE - 2] = crc & 0xFF;
  buf[EEPROM_CAL_SIZE - 1] = crc >> 8;

  for (uint16_t a = 0; a < EEPROM_CAL_SIZE; a++) {
    EEPROM.write(a + ee_addr, buf[a]);
  }

#if defined(ESP8266) || defined(ESP32)
  EEPROM.commit();
#endif
*/
  return true;
}

bool Adafruit_Sensor_Calibration_EEPROM::loadCalibration(void) {
  Serial.println(F("Load Cal"));
/*
  uint8_t buf[EEPROM_CAL_SIZE];

  uint16_t crc = 0xFFFF;
  for (uint16_t a = 0; a < EEPROM_CAL_SIZE; a++) {
    buf[a] = EEPROM.read(a + ee_addr);
    crc = crc16_update(crc, buf[a]);
  }

  if (crc != 0 || buf[0] != 0x75 || buf[1] != 0x54) {
    Serial.print("CRC: ");
    Serial.println(crc, HEX);
    return false;
  }
*/

  float offsets[16];
/*
  memcpy(offsets, buf + 2, 16 * 4);

  accel_zerog[0] = offsets[0];
  accel_zerog[1] = offsets[1];
  accel_zerog[2] = offsets[2];

  gyro_zerorate[0] = offsets[3];
  gyro_zerorate[1] = offsets[4];
  gyro_zerorate[2] = offsets[5];

  mag_hardiron[0] = offsets[6];
  mag_hardiron[1] = offsets[7];
  mag_hardiron[2] = offsets[8];

  mag_field = offsets[9];

  mag_softiron[0] = offsets[10];
  mag_softiron[1] = offsets[13];
  mag_softiron[2] = offsets[14];
  mag_softiron[3] = offsets[13];
  mag_softiron[4] = offsets[11];
  mag_softiron[5] = offsets[15];
  mag_softiron[6] = offsets[14];
  mag_softiron[7] = offsets[15];
  mag_softiron[8] = offsets[12];
*/
  // The below data was obtained using the MotionCal software tool.
  accel_zerog[0] = 0.0;
  accel_zerog[1] = 0.0;
  accel_zerog[2] = 0.0;

  gyro_zerorate[0] = 0.0;
  gyro_zerorate[1] = 0.0;
  gyro_zerorate[2] = 0.0;

  mag_hardiron[0] = -41.84;
  mag_hardiron[1] =  23.76;
  mag_hardiron[2] = -29.44;

  mag_field = 45.71;

  mag_softiron[0] =  0.993;
  mag_softiron[1] =  0.043;
  mag_softiron[2] = -0.014;
  mag_softiron[3] =  0.043;
  mag_softiron[4] =  1.019;
  mag_softiron[5] =  0.000;
  mag_softiron[6] = -0.014;
  mag_softiron[7] =  0.000;
  mag_softiron[8] =  0.991;

  return true;
}

bool Adafruit_Sensor_Calibration_EEPROM::printSavedCalibration(void) {
  Serial.println(F("Print saved cal"));
  Serial.println(F("------------"));
/*
  for (uint16_t a = ee_addr; a < ee_addr + EEPROM_CAL_SIZE; a++) {
    uint8_t c = EEPROM.read(a);
    Serial.print("0x");
    if (c < 0x10)
      Serial.print('0');
    Serial.print(c, HEX);
    Serial.print(", ");
    if ((a - ee_addr) % 16 == 15) {
      Serial.println();
    }
  }
*/
  Serial.println(F("\n------------"));
  return true;
}

#endif
My next analysis will be to test the processor's, sensor's and filter's response to a large Heading change. My initial testing indicates that this configuration will work out quite well. I will make a post with those results if they seem informative. Tinkering with the proportion or integral gains may improve the response... but I believe that what I have as-is may be good enough for now.

Again, THANKS for your help.

User avatar
sj_remington
 
Posts: 998
Joined: Mon Jul 27, 2020 4:51 pm

Re: Mahoney Filter Drift

Post by sj_remington »

Looks great! Good job.

User avatar
Autonomous_Sailboat
 
Posts: 31
Joined: Thu Dec 22, 2022 10:26 pm

Re: Mahoney Filter Drift

Post by Autonomous_Sailboat »

As noted above after much testing, the risk of magnetic compass 'drift' is reliant upon a good calibration. Unfortunately, I was unable to gain confidence that this issue would not become a failure as I could not count on my ability to get a 'perfect' calibration or that calibration would become 'lost' the sensor 'ages.' I need the ability to consistently rely upon a magnetic compass heading's accuracy.

What I have settled upon is a new AHRS sensor: the Adafruit 9-DOF Absolute Orientation IMU Fusion Breakout - BNO055 Product ID: 2472. It may be a discontinued item here at Adafruit... but its self-calibration capability is the answer to my problems at this time. All I have to do is power it up, shake it around a bit, and it self-identifies magnetic north with consistent reliability.

I hope this helps others that are experiencing the same concerns.

User avatar
sj_remington
 
Posts: 998
Joined: Mon Jul 27, 2020 4:51 pm

Re: Mahoney Filter Drift

Post by sj_remington »

its self-calibration capability is the answer to my problems at this time
Many people have found that the "self calibration" of the BNO055 sensor works so poorly as to be unacceptable. I gave up on it long ago. Example: viewtopic.php?f=19&t=112315

Keep in mind that all magnetometers should to be calibrated in their final resting place, and any time the immediate magnetic environment changes (e.g. construction changes, changes in current flow in nearby wires, etc.).

User avatar
Autonomous_Sailboat
 
Posts: 31
Joined: Thu Dec 22, 2022 10:26 pm

Re: Mahoney Filter Drift

Post by Autonomous_Sailboat »

sj_remington:

MANY thanks for the link. While the author of that forum post was concerned with the inaccuracy and backed that up with a couple of histogram charts... the test results confirmed for me that the BNO055 performance is going to be good enough for my application. My rough estimation is that his graphs depict roughly +/- 4 degree accuracy... which is may not be good enough for him (he desires +/-1 degree accuracy)... but it is good enough for sailing a boat. As long as it doesn't 'drift' I am good-to-go.

Many thanks for the quick response.

Locked
Please be positive and constructive with your questions and comments.

Return to “Other Products from Adafruit”