#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.