Complimentary filter con MPU6050 ed Arduino

Per terminare la serie, dopo Kalman e Madgwick un esempio di Complementary Filter
Dal punto di vistra computazionale e' il piu' semplice perche' si tratta di una sola formula
senza ricorsione e matrici

angle = 0.98 *(angle+gyro*dt) + 0.02*acc
(i parametri possono essere ottimizzati)

Una breve statistica (per raffronto con le prove precedenti)

Nr campioni : 20121
Media : -11.97
Range : 0.41
Std : 0.02
Varianza : 0.0005
Skewness :0.62

Il codice e' stato adattato da qui
-----------------------------------------------------------------------
// Complementary filter

#include <Wire.h>

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

float pitch=0;
float pitchAcc;
float P_CompCoeff= 0.98;
unsigned long delta_t,old_time;


void ComplementaryFilter(int ax,int ay,int az,int gy,int gz) {
 long squaresum=(long)ay*ay+(long)az*az;
 delta_t = micros()- old_time;
 old_time = micros();

 pitch+=((-gy/32.8f)*(delta_t/1000000.0f)); 
 pitchAcc =atan(ax/sqrt(squaresum))*RAD_TO_DEG;
 pitch =P_CompCoeff*pitch + (1.0f-P_CompCoeff)*pitchAcc;
}


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();
  old_time = micros();
}

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/131.0); Serial.print(",");
  Serial.print(gyRaw/131.0); Serial.print(",");
  Serial.println(gzRaw/131.0);*/
  ComplementaryFilter(axRaw,ayRaw,azRaw,gyRaw,gzRaw);
  Serial.println(pitch);
  delay(2);
}