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);
}