MPU9250 9DOF e filtro Madgwick

La MPU9250 e' un dispositivo con accelerometro, giroscopio e magnetometro triassiale

Per interfacciare l'MPU9250 ho usato la libreria https://github.com/sparkfun/MPU-9250_Breakout che funziona solo con schede Arduino con processore SAM (quindi MKR)

Nello sketch degli esempi e' stato aggiunto il filtro Madgwick

5852 dati

Pitch (filtrato)
media 60.475°
std 0.22°
skew -0.597

Roll (filtrato)
media 50.471°
std 0.34°
skew -0.113

Yaw (filtrato)
media 209.56°
std 0.54°
skew 0.2

---------------------------------------------------------------------------
#include <SparkFunMPU9250-DMP.h>
#include  <MadgwickAHRS.h> 



MPU9250_DMP imu;
Madgwick  MadgwickFilter;


void setup() 
{
  Serial.begin(115200);

   MadgwickFilter.begin(100);  // 100 Hz

  if (imu.begin() != INV_SUCCESS)
  {
    while (1)
    {
      Serial.println("Unable to communicate with MPU-9250");
      Serial.println("Check connections, and try again.");
      Serial.println();
      delay(5000);
    }
  }


  imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);


  imu.setGyroFSR(2000); // Set gyro to 2000 dps
  // Accel options are +/- 2, 4, 8, or 16 g
  imu.setAccelFSR(2); // Set accel to +/-2g
  // Note: the MPU-9250's magnetometer FSR is set at 
  // +/- 4912 uT (micro-tesla's)

  // setLPF() can be used to set the digital low-pass filter
  // of the accelerometer and gyroscope.
  // Can be any of the following: 188, 98, 42, 20, 10, 5
  // (values are in Hz).
  imu.setLPF(5); // Set LPF corner frequency to 5Hz

  // The sample rate of the accel/gyro can be set using
  // setSampleRate. Acceptable values range from 4Hz to 1kHz
  imu.setSampleRate(10); // Set sample rate to 10Hz

  // Likewise, the compass (magnetometer) sample rate can be
  // set using the setCompassSampleRate() function.
  // This value can range between: 1-100Hz
  imu.setCompassSampleRate(10); // Set mag rate to 10Hz

  imu.dmpBegin(DMP_FEATURE_GYRO_CAL |   // Enable gyro cal
              DMP_FEATURE_SEND_CAL_GYRO,// Send cal'd gyro values
              10);         
}

void loop() 
{

  if ( imu.dataReady() )
  {
    
    imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS);
    printIMUData();
  }
}

void printIMUData(void)
{  

  float accelX = imu.calcAccel(imu.ax);
  float accelY = imu.calcAccel(imu.ay);
  float accelZ = imu.calcAccel(imu.az);
  float gyroX = imu.calcGyro(imu.gx);
  float gyroY = imu.calcGyro(imu.gy);
  float gyroZ = imu.calcGyro(imu.gz);
  float magX = imu.calcMag(imu.mx);
  float magY = imu.calcMag(imu.my);
  float magZ = imu.calcMag(imu.mz);

  MadgwickFilter.update(accelX,accelY,accelZ,gyroX,gyroY,gyroZ,magX,magY,magZ) ;

  
  Serial.print(String(accelX) + "," +String(accelY) + "," + String(accelZ));
  Serial.print("," + String(gyroX) + "," + String(gyroY) + "," + String(gyroZ) + ",");
  Serial.print(String(magX) + "," + String(magY) + "," + String(magZ)+ "," );
  Serial.print(String(MadgwickFilter.getPitch())+ "," +String(MadgwickFilter.getRoll())+ "," +String(MadgwickFilter.getYaw()));
  Serial.println();
}

Filtro Madgwick con MPU6050 ed Arduino

Un test del filtro di Madgwick , un possibile sostituto per il filtro Kalman su processori meno potenti

La libreria per Arduino si trova a questo link

Da notare prima di tutte che :

1) la finestra di campionamento del filtro in Madgwick.begin aumenta o diminuisce la velocita' di convergenza dei dati

2) i dati devono essere inseriti nel filtro con le corrette unita' fisiche (m/sec2 e deg/sec) e non con i dati raw in uscita dal sensore

3) la libreria permette di inserire anche i dati di un sensore magnetico (che non e' pero' presente nella 6050)

Una prova veloce con il filtro Madgwick e MPU statica

Nr campioni : 73071
Media : 9.82
Range : 0.56
Std : 0.04
Varianza : 0.001
Skewness : -0.01

Nelle stesse condizioni ma con filtro Kalman

Nr campioni : 16568
Media : 9.84
Range : 0.44
Std : 0.04
Varianza : 0.001
Skewness : 0.05

Aggiornamento : per calcolare il valore della Temperatura in gradi Celsius si usa la formula

T = (lettura/340.0)+36.53

--------------------------------------------------------------------------------------------
#include  <MadgwickAHRS.h> 
#include <Wire.h>

int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, temperature;


Madgwick  MadgwickFilter;

void setup() {
Serial.begin(115200);
  //Inizio configurazione clock interno 
  
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  
  // Inizializzazione Accelerometro intervallo di 8g Fattore di scala 4096
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);
  Wire.write(0x10);
  Wire.endTransmission();
  
  // Inizializzazione Gyro Intervallo di 500 deg/sec LSB (fattore di scala) 65.5 deg/s
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x08);
  Wire.endTransmission();

  // Inizializzazione LPF (Filtro passa basso) 43 Hz
  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x03);
  Wire.endTransmission();
  
  MadgwickFilter.begin(100);  // 100 Hz

}

void loop() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 14);
  while (Wire.available() < 14);
  axRaw = Wire.read() << 8 | Wire.read();
  ayRaw = Wire.read() << 8 | Wire.read();
  azRaw = Wire.read() << 8 | Wire.read();
  temperature = Wire.read() << 8 | Wire.read();
  gxRaw = Wire.read() << 8 | Wire.read();
  gyRaw = Wire.read() << 8 | Wire.read();
  gzRaw = Wire.read() << 8 | Wire.read();

  /*Serial.print(axRaw/4096.0); Serial.print(",");
  Serial.print(ayRaw/4096.0); Serial.print(",");
  Serial.print(azRaw/4096.0); Serial.print(",");
  Serial.print(gxRaw/65.5); Serial.print(",");
  Serial.print(gyRaw/65.5); Serial.print(",");
  Serial.println(gzRaw/65.5);*/
  MadgwickFilter.updateIMU(gxRaw/65.5,gyRaw/65.5,gzRaw/65.5,axRaw/4096.0,ayRaw/4096.0,azRaw/4096.0) ;
  Serial.println(MadgwickFilter.getRoll());
  //ROLL  = MadgwickFilter.getRoll();
  //PITCH = MadgwickFilter.getPitch();
  //YAW   = MadgwickFilter.getYaw();
  delay(2);
}