Low Pass Filter | Technology Tutorials

In today’s video lesson we show how you can remove the vibration noise from your roll and pitch values using a low pass filter. The filter removes the high frequency noise resulting from vibrations.

MPU6050 — Schematic for connecting the GY-87 module to the Arduino

Below is the code we developed in this lesson.

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
float Ax;
float Ay;
float Az;

// If You want Calibrated results, you must rotate
// your board to all possible orientations, and 
// record your velues of xMax, xMin, yMax, yMin
// zMax, zMin. I show you how to do that in LESSON
// 79 on my youtube channel. Put in your values and then
// uncomment the code below. Don't use my values, you have
// to measure your own for it to work.

// float xMax= 1.03;
// float xMin= -.97;
// float yMax= .98;
// float yMin= -1.01;
// float zMax=.96 ;
// float zMin= -1.08;

// float xOffset = (xMax+xMin)/2;
// float yOffset = (yMax+yMin)/2;
// float zOffset = (zMax+zMin)/2;

// float xScale = 2/(xMax -xMin);
// float yScale = 2/(yMax - yMin);
// float zScale = 2/(zMax -zMin);

float roll=0;
float pitch=0;
float rollRAW;
float pitchRAW;

Adafruit_MPU6050 mpu;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  mpu.begin();
  Serial.println("MPU6050 Started!");
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);


}

void loop() {
  // put your main code here, to run repeatedly:
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  Ax=a.acceleration.x/9.81;
  Ay=a.acceleration.y/9.81;
  Az=a.acceleration.z/9.81;

//Uncomment these lines of code to do the callibration
//This will only work if you have measured your max
//and min values, and put them in at the top of the code
  // Ax = xScale*(Ax-xOffset);
  // Ay = yScale*(Ay-yOffset);
  // Az = zScale*(Az-zOffset);

  pitchRAW = atan2(Ay,sqrt(Az*Az+Ax*Ax))*360/(2*3.14);
  rollRAW = atan2(Ax,sqrt(Az*Az+Ay*Ay))*360/(2*3.14);

  pitch = .75*pitch + .25*pitchRAW;
  roll = .75*roll + .25*rollRAW;

  Serial.print("RollRAW:");
  Serial.print(rollRAW);
  Serial.print(',');
  Serial.print("PitchRAW:");
  Serial.print(pitchRAW);
    Serial.print(',');
  Serial.print("Roll:");
  Serial.print(roll);
  Serial.print(',');
  Serial.print("Pitch:");
  Serial.print(pitch);
    Serial.print(',');
   Serial.print("UL:");
 Serial.print(90);
 Serial.print(',');
 Serial.print("LL:");
 Serial.println(-90);



//   Serial.print("Ax:");
//   Serial.print(Ax);
//   Serial.print(',');
//   Serial.print("Ay:");
//   Serial.print(Ay);
//   Serial.print(',');
//     Serial.print("Az:");
//   Serial.print(Az);
//   Serial.print(',');
//  Serial.print("UL:");
//  Serial.print(1);
//  Serial.print(',');
//  Serial.print("LL:");
//  Serial.println(-1);

  delay(50);

}

100

101

102

103

104

105

106

107

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

float Ax;

float Ay;

float Az;

// If You want Calibrated results, you must rotate

// your board to all possible orientations, and

// record your velues of xMax, xMin, yMax, yMin

// zMax, zMin. I show you how to do that in LESSON

// 79 on my youtube channel. Put in your values and then

// uncomment the code below. Don't use my values, you have

// to measure your own for it to work.

// float xMax= 1.03;

// float xMin= -.97;

// float yMax= .98;

// float yMin= -1.01;

// float zMax=.96 ;

// float zMin= -1.08;

// float xOffset = (xMax+xMin)/2;

// float yOffset = (yMax+yMin)/2;

// float zOffset = (zMax+zMin)/2;

// float xScale = 2/(xMax -xMin);

// float yScale = 2/(yMax - yMin);

// float zScale = 2/(zMax -zMin);

float roll=0;

float pitch=0;

float rollRAW;

float pitchRAW;

Adafruit_MPU6050 mpu;

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

mpu.begin();

Serial.println("MPU6050 Started!");

mpu.setAccelerometerRange(MPU6050_RANGE_2_G);

mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

}

void loop() {

// put your main code here, to run repeatedly:

sensors_event_t a, g, temp;

mpu.getEvent(&a, &g, &temp);

Ax=a.acceleration.x/9.81;

Ay=a.acceleration.y/9.81;

Az=a.acceleration.z/9.81;

//Uncomment these lines of code to do the callibration

//This will only work if you have measured your max

//and min values, and put them in at the top of the code

// Ax = xScale*(Ax-xOffset);

// Ay = yScale*(Ay-yOffset);

// Az = zScale*(Az-zOffset);

pitchRAW = atan2(Ay,sqrt(Az*Az+Ax*Ax))*360/(2*3.14);

rollRAW = atan2(Ax,sqrt(Az*Az+Ay*Ay))*360/(2*3.14);

pitch = .75*pitch + .25*pitchRAW;

roll = .75*roll + .25*rollRAW;

Serial.print("RollRAW:");

Serial.print(rollRAW);

Serial.print(',');

Serial.print("PitchRAW:");

Serial.print(pitchRAW);

Serial.print(',');

Serial.print("Roll:");

Serial.print(roll);

Serial.print(',');

Serial.print("Pitch:");

Serial.print(pitch);

Serial.print(',');

Serial.print("UL:");

Serial.print(90);

Serial.print(',');

Serial.print("LL:");

Serial.println(-90);

// Serial.print("Ax:");

// Serial.print(Ax);

// Serial.print(',');

// Serial.print("Ay:");

// Serial.print(Ay);

// Serial.print(',');

// Serial.print("Az:");

// Serial.print(Az);

// Serial.print(',');

// Serial.print("UL:");

// Serial.print(1);

// Serial.print(',');

// Serial.print("LL:");

// Serial.println(-1);

delay(50);

}

<span data-mce-type="bookmark" style="display: inline-block; width: 0px; overflow: hidden; line-height: 0;" class="mce_SELRES_start"></span>
In this project, we have shown how accelerometers can be used to approximate tilt. We showed in Lesson 6 how vibration can cause a problem with such a strategy, as the system will interpret vibration to be changes in tilt. A partial solution to this problem can be to program a low pass filter into the project. The video above introduces you to the concept of a low pass filter, and shows some simple examples of programming and using a low pass filter to reduce noise.

The code below is for demo purposes only, and should not be used in any real applications. It just demonstrates how to work with this sensor in benchtop presentations.

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;
#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
}

void loop() {
  // put your main code here, to run repeatedly:
uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);
imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiFnew=.95*phiFold+.05*phiM;
thetaFnew=.95*thetaFold+.05*thetaM;
Serial.print(acc.x()/9.8);
Serial.print(",");
Serial.print(acc.y()/9.8);
Serial.print(",");
Serial.print(acc.z()/9.8);
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.print(system);
Serial.print(",");
Serial.print(thetaM);
Serial.print(",");
Serial.print(phiM);
Serial.print(",");
Serial.print(thetaFnew);
Serial.print(",");
Serial.println(phiFnew);

phiFold=phiFnew;
thetaFold=thetaFnew;

 
delay(BNO055_SAMPLERATE_DELAY_MS);
}

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

float thetaM;

float phiM;

float thetaFold=0;

float thetaFnew;

float phiFold=0;

float phiFnew;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

myIMU.begin();

delay(1000);

int8_t temp=myIMU.getTemp();

myIMU.setExtCrystalUse(true);

}

void loop() {

// put your main code here, to run repeatedly:

uint8_t system, gyro, accel, mg = 0;

myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);

thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiFnew=.95*phiFold+.05*phiM;

thetaFnew=.95*thetaFold+.05*thetaM;

Serial.print(acc.x()/9.8);

Serial.print(",");

Serial.print(acc.y()/9.8);

Serial.print(",");

Serial.print(acc.z()/9.8);

Serial.print(",");

Serial.print(accel);

Serial.print(",");

Serial.print(gyro);

Serial.print(",");

Serial.print(mg);

Serial.print(",");

Serial.print(system);

Serial.print(",");

Serial.print(thetaM);

Serial.print(",");

Serial.print(phiM);

Serial.print(",");

Serial.print(thetaFnew);

Serial.print(",");

Serial.println(phiFnew);

phiFold=phiFnew;

thetaFold=thetaFnew;

delay(BNO055_SAMPLERATE_DELAY_MS);

}

Making The World a Better Place One High Tech Project at a Time. Enjoy!

Technology Tutorials

Tag Archives: Low Pass Filter

Applying a Low Pass Filter to Roll and Pitch Calculations on the MPU6050

9-Axis IMU LESSON 7: Understanding Low Pass Filters

Making The World a Better Place One High Tech Project at a Time. Enjoy!