Complimentary Filter | Technology Tutorials

In this video lesson we show how to create a complimentary filter such we get pitch and roll data from the MPU6050 which is quick and responsive, accurate, and low noise. We are using the following schematic:

This is the code we developed in the video.

from imu import MPU6050
from machine import I2C,Pin
import math
import time
 
i2c=I2C(0, sda=Pin(16), scl=Pin(17), freq=400000)
mpu = MPU6050(i2c)
 
rollG=0
pitchG=0

rollComp=0
pitchComp=0

yaw=0
tLoop=0
cnt=0
 
while True:
    tStart=time.ticks_ms()
    
    xGyro=mpu.gyro.x
    yGyro=-mpu.gyro.y
    zGyro=mpu.gyro.z
    
    xAccel=mpu.accel.x
    yAccel=mpu.accel.y
    zAccel=mpu.accel.z
    
    rollG=rollG+yGyro*tLoop
    pitchG=pitchG+xGyro*tLoop
    
    rollA=math.atan(xAccel/zAccel)/2/math.pi*360
    pitchA=math.atan(yAccel/zAccel)/2/math.pi*360
    
    rollComp= rollA*.005 + .995*(rollComp+yGyro*tLoop)
    pitchComp= pitchA*.005 + .995*(pitchComp+xGyro*tLoop)
    
    cnt=cnt+1
    if cnt==10:
        cnt=0
        print('RA: ',rollA,'PA: ',pitchA,'RC: ',rollComp,'PC: ',pitchComp)
    tStop=time.ticks_ms()
    tLoop=(tStop-tStart)*.001

from imu import MPU6050

from machine import I2C,Pin

import math

import time

i2c=I2C(0, sda=Pin(16), scl=Pin(17), freq=400000)

mpu = MPU6050(i2c)

rollG=0

pitchG=0

rollComp=0

pitchComp=0

yaw=0

tLoop=0

cnt=0

while True:

tStart=time.ticks_ms()

xGyro=mpu.gyro.x

yGyro=-mpu.gyro.y

zGyro=mpu.gyro.z

xAccel=mpu.accel.x

yAccel=mpu.accel.y

zAccel=mpu.accel.z

rollG=rollG+yGyro*tLoop

pitchG=pitchG+xGyro*tLoop

rollA=math.atan(xAccel/zAccel)/2/math.pi*360

pitchA=math.atan(yAccel/zAccel)/2/math.pi*360

rollComp= rollA*.005 + .995*(rollComp+yGyro*tLoop)

pitchComp= pitchA*.005 + .995*(pitchComp+xGyro*tLoop)

cnt=cnt+1

if cnt==10:

cnt=0

print('RA: ',rollA,'PA: ',pitchA,'RC: ',rollComp,'PC: ',pitchComp)

tStop=time.ticks_ms()

tLoop=(tStop-tStart)*.001

In this lesson we show how a complimentary filter can be used to create the best of both worlds in approximating tilt from data coming from the accelerometers and gyros. The goal of our complimentary filter is to weight our overall result to take advantage the short term accuracy of the gyro measurement, and combine it with the long term stability of the accelerometer. In effect the combination is applying a high pass filter to the gyro and a low pass filter to the accelerometer. Remember that the angles are approximate, and were developed assuming roll and pitch stay under 45 degrees. The trigonometry becomes more difficult when considering higher tilt angles.

To play along at home, you will need an Arduino Nano, and an Adafruit BNO055 Inertial Measurement Sensor.

The code below is for demo purposes only, and should not be used in any real applications. It just demonstrates how a complimentary filter works. Of course, any real systems need to go into much more depth than the simple demo project below.

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

float thetaG=0;
float phiG=0;

float theta;
float phi;

float dt;
unsigned long millisOld;

#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);
millisOld=millis();
}

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);
imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

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;

dt=(millis()-millisOld)/1000.;
millisOld=millis();
theta=(theta+gyr.y()*dt)*.95+thetaM*.05;
phi=(phi-gyr.x()*dt)*.95+ phiM*.05;
thetaG=thetaG+gyr.y()*dt;
phiG=phiG-gyr.x()*dt;

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.print(phiFnew);

Serial.print(",");
Serial.print(thetaG);
Serial.print(",");
Serial.print(phiG);
Serial.print(",");
Serial.print(theta);
Serial.print(",");
Serial.println(phi);

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;

float thetaG=0;

float phiG=0;

float theta;

float phi;

float dt;

unsigned long millisOld;

#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);

millisOld=millis();

}

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);

imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

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;

dt=(millis()-millisOld)/1000.;

millisOld=millis();

theta=(theta+gyr.y()*dt)*.95+thetaM*.05;

phi=(phi-gyr.x()*dt)*.95+ phiM*.05;

thetaG=thetaG+gyr.y()*dt;

phiG=phiG-gyr.x()*dt;

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.print(phiFnew);

Serial.print(",");

Serial.print(thetaG);

Serial.print(",");

Serial.print(phiG);

Serial.print(",");

Serial.print(theta);

Serial.print(",");

Serial.println(phi);

phiFold=phiFnew;

thetaFold=thetaFnew;

delay(BNO055_SAMPLERATE_DELAY_MS);

}

Technology Tutorials

Tag Archives: Complimentary Filter

Improving Accuracy of MPU6050 Data Using a Complimentary Filter

9-Axis IMU LESSON 9: Accurate and Stable Tilt Using Accelerometers, Gyros and a Complimentary Filter

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