Mahony with calibration

andreaarcarisi · April 30, 2024, 10:05am

Hi everyone. I'm recently trying to write a mahony filter for use with a MPU6050 + QMC5883L. at the moment (with some uncertainty) I have written a code to extrapolate the raw data of the sensors. then on the MPU6050 I average the bias to be subtracted and then through an algorithm on matlab I calibrate the QMC5883L sensor.

Now I'm wondering two things:

do you think the calibration of the MPU6050 is correct? I have doubts looking at the serial data based on how I rotate the sensor I have higher values in some axes than 9.81 as an average. so I'm wondering if there already exists a file that correctly calibrates the mpu6050.
inserting the mahony filter, after having done the calibration correctly, what and how to fine-tune the filter? according to what criteria?

#include <Wire.h>
#include <math.h>

#define MPU6050_ADDRESS 0x68
#define MPU6050_REG_ACCEL_XOUT_H 0x3B
#define MPU6050_REG_GYRO_XOUT_H 0x43

#define QMC5883L_ADDRESS 0x0D
#define QMC5883L_REG_X_LSB 0x00
#define QMC5883L_REG_Y_LSB 0x02
#define QMC5883L_REG_Z_LSB 0x04

float accBiasX = 0, accBiasY = 0, accBiasZ = 0;
float gyroBiasX = 0, gyroBiasY = 0, gyroBiasZ = 0;

void setup() {
  Wire.begin();
  Serial.begin(115200);
  
pinMode(2, OUTPUT);
  // Inizializza il MPU6050
  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(0x6B); // Power Management register
  Wire.write(0);    // Accendi il dispositivo (wakes up the MPU-6050)
  Wire.endTransmission(true);
  
  // Inizializza il sensore QMC5883L
  Wire.beginTransmission(QMC5883L_ADDRESS);
  Wire.write(0x09); // Registri di controllo
  Wire.write(0x1D); // Modalità continuativa con 50Hz
  Wire.endTransmission();
  
  // Calibra l'MPU6050
  calibrateMPU6050();

  // Lampeggia il buzzer due volte

}

void loop() {
  int16_t accX, accY, accZ;
  int16_t gyroX, gyroY, gyroZ;
  int16_t magX, magY, magZ;
  
  // Leggi i dati dell'accelerometro dal MPU6050
  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(MPU6050_REG_ACCEL_XOUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDRESS, 6);
  if (Wire.available() >= 6) {
    accX = Wire.read() << 8 | Wire.read();
    accY = Wire.read() << 8 | Wire.read();
    accZ = Wire.read() << 8 | Wire.read();
  }
  
  // Leggi i dati del giroscopio dal MPU6050
  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(MPU6050_REG_GYRO_XOUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDRESS, 6);
  if (Wire.available() >= 6) {
    gyroX = Wire.read() << 8 | Wire.read();
    gyroY = Wire.read() << 8 | Wire.read();
    gyroZ = Wire.read() << 8 | Wire.read();
  }
  
  // Leggi i dati del magnetometro dal QMC5883L
  Wire.beginTransmission(QMC5883L_ADDRESS);
  Wire.write(QMC5883L_REG_X_LSB);
  Wire.endTransmission(false);
  Wire.requestFrom(QMC5883L_ADDRESS, 6);
  if (Wire.available() >= 6) {
    magX = Wire.read() | (Wire.read() << 8);
    magY = Wire.read() | (Wire.read() << 8);
    magZ = Wire.read() | (Wire.read() << 8);
  }
  
  // Calibra e converte i dati dell'MPU6050
  float accX_calibrated = ((float)accX - accBiasX) / 16384.0 * 9.81;
  float accY_calibrated = ((float)accY - accBiasY) / 16384.0 * 9.81;
  float accZ_calibrated = ((float)accZ - accBiasZ) / 16384.0 * 9.81;
  float gyroX_calibrated = ((float)gyroX - gyroBiasX) / 131.0;
  float gyroY_calibrated = ((float)gyroY - gyroBiasY) / 131.0;
  float gyroZ_calibrated = ((float)gyroZ - gyroBiasZ) / 131.0;
  
   // Creazione della stringa di output
  char output[100]; // Assicurati che la dimensione sia sufficiente per contenere la stringa
  sprintf(output, "&,%f,%f,%f,%f,%f,%f,%d,%d,%d$", accX_calibrated, accY_calibrated, accZ_calibrated, gyroX_calibrated, gyroY_calibrated, gyroZ_calibrated, magX, magY, magZ);
  
  // Stampa la stringa su Serial
  Serial.println(output);
  
  delay(10); // Aggiorna ogni 10 ms
}

void calibrateMPU6050() {
  unsigned long startTime = millis();
  unsigned long duration = 11000; // Acquisizione dei dati per 5 secondi
  
  long accX_sum = 0, accY_sum = 0, accZ_sum = 0;
  long gyroX_sum = 0, gyroY_sum = 0, gyroZ_sum = 0;
  int count = 0;
  
  while (millis() - startTime < duration) {
    int16_t accX, accY, accZ;
    int16_t gyroX, gyroY, gyroZ;
    
    // Leggi i dati dell'accelerometro dal MPU6050
    Wire.beginTransmission(MPU6050_ADDRESS);
    Wire.write(MPU6050_REG_ACCEL_XOUT_H);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU6050_ADDRESS, 6);
    if (Wire.available() >= 6) {
      accX = Wire.read() << 8 | Wire.read();
      accY = Wire.read() << 8 | Wire.read();
      accZ = Wire.read() << 8 | Wire.read();
      accX_sum += accX;
      accY_sum += accY;
      accZ_sum += accZ;
    }
    
    // Leggi i dati del giroscopio dal MPU6050
    Wire.beginTransmission(MPU6050_ADDRESS);
    Wire.write(MPU6050_REG_GYRO_XOUT_H);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU6050_ADDRESS, 6);
    if (Wire.available() >= 6) {
      gyroX = Wire.read() << 8 | Wire.read();
      gyroY = Wire.read() << 8 | Wire.read();
      gyroZ = Wire.read() << 8 | Wire.read();
      gyroX_sum += gyroX;
      gyroY_sum += gyroY;
      gyroZ_sum += gyroZ;
    }
    
    count++;
    delay(100); // Acquisisce i dati ogni 10 ms
  }
  
  // Calcola il bias error per l'accelerometro e il giroscopio
  accBiasX = (float)accX_sum / count;
  accBiasY = (float)accY_sum / count;
  accBiasZ = ((float)accZ_sum / count) - 16384.0; // 16384 è il valore medio quando l'MPU6050 è fermo
  gyroBiasX = (float)gyroX_sum / count;
  gyroBiasY = (float)gyroY_sum / count;
  gyroBiasZ = (float)gyroZ_sum / count;

  for (int i = 0; i < 2; i++) {
  digitalWrite(2, HIGH);
  delay(100);
  digitalWrite(2, LOW);
  delay(100);
}
}

gcjr · April 30, 2024, 11:04am

what is a "mahory" filter ?

                       (i could, why didn't you)?

andreaarcarisi · April 30, 2024, 11:09am

sorry, Mahony filter. The Mahony filter is designed to estimate the orientation of a vehicle or moving object using raw measurements from acceleration sensors and gyroscopes

jremington · April 30, 2024, 2:00pm

Accelerometers and magnetometers can be calibrated in exactly the same way, as described in this tutorial:

The Mahony filter won't work unless the coordinate axes of the magnetometer and accelerometer are both right handed and are accurately aligned with respect to each other. That is, the sensors' X, Y and Z axes must point in the same directions.

andreaarcarisi · April 30, 2024, 2:35pm

thank you very much. instead I was wondering how to calibrate the mahony? Is it trial and error or is there a procedure?

andreaarcarisi · April 30, 2024, 2:37pm

also I was wondering if looking in my code, I'm acquiring the raw data correctly which is my main doubt. whether I should use some different registry setting or not.

jremington · April 30, 2024, 3:35pm

The procedure is described in the tutorial linked in post #4.

andreaarcarisi · April 30, 2024, 3:43pm

perfect thanks, last thing, forgive me if I insist, could you just check that the raw data acquisition is exact and set correctly? I have serious doubts you did it the right way, please

jremington · April 30, 2024, 3:45pm

Sorry, I have no idea what you are talking about.

andreaarcarisi · April 30, 2024, 3:52pm

in post #1 I uploaded the code I use, I'm not sure I used the correct codes to acquire the acc,gyro,mag data.

jremington · April 30, 2024, 3:58pm

I've never used the QMC5883L.

Here is what I have used to obtain raw data from the MPU-6050:

// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>
const int MPU_addr = 0x68; // I2C address of the MPU-6050
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

void setup() {
  Wire.begin();
  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);
  Serial.begin(9600);
}

void loop() {
  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
  int16_t t = Wire.read();
  AcX = (t << 8) | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  t = Wire.read();
  AcY = (t << 8) | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  t = Wire.read();
  AcZ = (t << 8) | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  t = Wire.read();
  Tmp = (t << 8) | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  t = Wire.read();
  GyX = (t << 8) | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  t = Wire.read();
  GyY = (t << 8) | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  t = Wire.read();
  GyZ = (t << 8) | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  t = Wire.read();
  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.print(AcZ);
  Serial.print(" | Tmp = "); Serial.print(Tmp / 340.00 + 36.53); //equation for temperature in degrees C from datasheet
  Serial.print(" | GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
  delay(333);
}

NOTE: In C++, the following construction is not guaranteed to work as you expect (the byte order may be wrong):

     accX = Wire.read() << 8 | Wire.read();

andreaarcarisi · April 30, 2024, 4:01pm

thanks a lot

andreaarcarisi · April 30, 2024, 10:48pm

one last curiosity, then what scale factor do they use to convert the acc and gyro data? I would like to send the full scale to 16g on the acc and 2000°/s on the gyroscope, but I don't know how to set it or how to correctly convert the raw data.

jremington · April 30, 2024, 11:36pm

Who are "they"?

You can set those scale factors to anything you want, but the Mahony filter requires gyro data to be in radians/second and the accelerometer data to be normalized (always have magnitude 1, as it defines only the vertical direction: Up/Down).

system · October 27, 2024, 11:36pm

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

Topic		Replies	Views
Mahony calibration problem Programming	2	160	November 4, 2024
Low pass filter for Accelerometer, high pass for Gyroscope? General Guidance	4	9030	May 5, 2021
MPU6050 help Sensors	3	1145	May 6, 2021
Calibration of MPU6050 Programming	2	1107	December 21, 2021
MPU6050 rotation problem using MahonyAHRS Sensors	1	1619	May 6, 2021

Mahony with calibration

Related topics