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