MPU6050 Vibration noise filtering - DLPF and Kalman Filter


#include <MPU6050_tockn.h>
#include <Wire.h>
#include <KalmanFilter.h>

//typedef union{
//  float number;
//  uint8_t bytes[4];
//} FLOATUNION_t;

//FLOATUNION_t myValue;

MPU6050 mpu6050(Wire,0.02,0.998);
 
KalmanFilter kalmanX(0.0005, 0.005, 0.1);  //(Q_angle, Q_bais, R)
KalmanFilter kalmanY(0.0005, 0.005, 0.1);

float accPitch = 0;
float accRoll = 0;

float kalPitch = 0;
float kalRoll = 0;

float roll = 0.0;

void setup() 
{
  Serial.begin(115200);
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);

  while (!Serial)
  {
    ; // wait for serial port to connect. Needed for native USB port only
  }
}

void loop() 
{
  mpu6050.update();
  kalRoll = (kalmanX.update(mpu6050.getAccAngleX(), mpu6050.getGyroX()));
  Serial.println(kalRoll,1);
  //Serial.print("\t");
  //Serial.print(mpu6050.getRawAccX());
  //Serial.print("\t");
  //Serial.print(mpu6050.getRawAccY());
  //Serial.print("\t");
  //Serial.print(mpu6050.getRawAccZ());
  //Serial.print("\t");
  //Serial.print(mpu6050.getRawGyroX());
  //Serial.print("\t");
  //Serial.print(mpu6050.getRawGyroY());
  //Serial.print("\t");
  //Serial.println(mpu6050.getRawGyroZ());
  
  
  

}

I have used Tockn library and kalman filter library. I have added a line in tockn library to define the DLPF. The kalman coefficient (Q, Q_bais, R ) are chosen based on experimentation at static conditions.