#include "I2Cdev.

h"
#include "MPU6050.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 acelerometro;
int16_t ax, ay, az;
#define OUTPUT_READABLE_ACCELGYRO
// Declaracion de las variables de estado
float a_0 = 1,a_1 = -1.9556, a_2 = 0.9565;
float b_0 = 0.0002414, b_1 = 0.0004827, b_2 = 0.0002414;
float Xx_0 = 0, Xx_1 = 0 , Xx_2 = 0;
float Xy_0 = 0, Xy_1 = 0 , Xy_2 = 0;
float Yx_0 = 0, Yx_1 = 0 , Yx_2 = 0;
float Yy_0 = 0, Yy_1 = 0 , Yy_2 = 0;
float Zx_0 = 0, Zx_1 = 0 , Zx_2 = 0;
float Zy_0 = 0, Zy_1 = 0 , Zy_2 = 0;
void setup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
Serial.println("Initializing I2C devices...");
acelerometro.initialize();
Serial.println("Testing device connections...");
Serial.println(acelerometro.testConnection() ? "MPU6050 connection successfu
l" : "MPU6050 connection failed");
}
void loop() {
//
unsigned long t =millis();
acelerometro.getAcceleration(&ax, &ay, &az);
#ifdef OUTPUT_READABLE_ACCELGYRO
// display tab-separated accel/gyro x/y/z values
filtroX();
filtroY();
filtroZ();
Serial.print(ax); Serial.print(",");
Serial.print(ay); Serial.print(",");
Serial.print(az); Serial.print(",");
Serial.print(Xy_2); Serial.print(",");
Serial.print(Yy_2); Serial.print(",");
Serial.println(Zy_2);
#endif
//
unsigned long t1 =millis();
//
t1=t1-t;
//
Serial.println(t1);
}

void filtroX(){
Xx_0 = Xx_1;
Xx_1 = Xx_2;
Xx_2 = ax;
Xy_0 = Xy_1;
Xy_1 = Xy_2;
Xy_2 = b_0*Xx_2+b_1*Xx_1+b_2*Xx_0-a_1*Xy_1-a_2*Xy_0;
}
void filtroY(){
Yx_0 = Yx_1;
Yx_1 = Yx_2;
Yx_2 = ay;
Yy_0 = Yy_1;
Yy_1 = Yy_2;
Yy_2 = b_0*Yx_2+b_1*Yx_1+b_2*Yx_0-a_1*Yy_1-a_2*Yy_0;
}
void filtroZ(){
Zx_0 = Zx_1;
Zx_1 = Zx_2;
Zx_2 = az;
Zy_0 = Zy_1;
Zy_1 = Zy_2;
Zy_2 = b_0*Zx_2+b_1*Zx_1+b_2*Zx_0-a_1*Zy_1-a_2*Zy_0;
}