Go Down

Topic: Filtering raw data from MPU6050 6DOF Gyroscope  (Read 184 times) previous topic - next topic

Spange

Hello all,
I have a MPU6050 6DOF Gyroscope  which outputs data to the serial monitor along with many other data. The data is read by Matlab.

I notice that the raw data is quite noisy. It varies a lot even then its at rest.

Since the data is sent to Matlab i'd like Arduino to write to the serial monitor at least at 2 Hz,
so I'm not so keen on (e.g.) logging 10 values, averaging them and then outputting it.

Is there a way to implement a filter (e.g. low pass filter) that only needs one value as input (because it remembers previous data)?

What is the fastest solution?

Thank you!

This is an excerpt of my code. It does a lot more, communicating with other sensors ++

Code: [Select]

const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp;

void setup() {

  Serial.begin(9600);

  // MPU
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

}

void loop() {

  // MPU
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)   
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  //Serial.print("AcX = "); Serial.print(AcX/16384.0); Serial.print("g");            // +-2g over 16bit
  //Serial.print(" | AcY = "); Serial.print(AcY/16384.0); Serial.print("g");
  //Serial.print(" | AcZ = "); Serial.print(AcZ/16384.0); Serial.print("g");
  //Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); Serial.println("C");  //equation for temperature in degrees C from datasheet

}



Paul_KD7HB

A low pass filter is easy! Just ignore any value great than your cutoff value.

Paul

Spange

I found a quite simple LP from Wikipedia, i think it works fine (see below).

What I was aiming for with my post was to get som opinions on:
- Have anybody else used the same gyroscope?
- If so: what did you use a cutoff frequency and sampling time?
- Have anybody tried to calibrate it, how do I do that?

Code: [Select]

  // Filtering, simple LP
  AcX = (1-alpha)*AcX + alpha*u1;
  AcY = (1-alpha)*AcY + alpha*u2;
  AcZ = (1-alpha)*AcZ + alpha*u3;
  Tmp = (1-alpha)*Tmp + alpha*u4;

MarkT

Single pole digital filter:

Code: [Select]


#define ALPHA 0.1   // or whatever, smaller is slower changing
float AcX_average, AcY_average, AcZ_average, Tmp_average ;


//Then after getting your AcX, AcY etc readings:

  AcX_average += ALPHA * (AcX - AcX_average) ; // 1st order digital filter
  AcY_average += ALPHA * (AcY - AcY_average) ; // 1st order digital filter
  // etc, and pass the averaged values back


[ My version uses only one multiply ]
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

Power_Broker

What I was aiming for with my post was to get som opinions on:
- Have anybody else used the same gyroscope?
- If so: what did you use a cutoff frequency and sampling time?
- Have anybody tried to calibrate it, how do I do that?
I've use the gryo often and have had no problems with it.

Your problem is quite strange because gyros usually have drift (which is a low frequency noise) NOT high frequency noise like you mention. I think whatever noise you are seeing is probably negligible. What are you using it for? Can you provide test data/graphs?

Usually when I use an IMU such as the MPU6050, it is to calculate orientation Euler Angles with a complimentary filter. This uses BOTH gyro and accellerometer data simultaneously. Here is my code (for an MPU 9250):

Code: [Select]
#include <Wire.h>
#include <FaBo9Axis_MPU9250.h>
#include <Servo.h>

#define M_PI 3.14159265359

FaBo9Axis fabo_9axis;

float offsetax = 0;
float offsetay = 0;
float ax, ay, az;
float gx, gy, gz;
float integrated_gx = 0;
float scaled_gx = 0;
float integrated_gy = 0;
float scaled_gy = 0;
float angleRoll = 0;
float anglePitch = 0;
float A_ = 0.7;
float dt = 0.01;
float angleRollAcc = 0;
float anglePitchAcc = 0;
byte gyroSensitivity = 131; //16 bit ADC with +-250dps --> sensitivity = 2^16 / 500 = 131

double currentTime = millis();
double timeBench = currentTime;
double samplePeriod = 10;  //sample period in ms

Servo horizontal;
Servo vertical;



void setup()
{
  Serial.begin(115200);

  if (fabo_9axis.begin())
  {
    Serial.println("configured FaBo 9Axis I2C Brick");
  }
  else
  {
    Serial.println("device error");
    while (1);
  }

  horizontal.attach(30);
  vertical.attach(31);

  horizontal.write(90);
  vertical.write(90);
}



void loop()
{
  //prime the timer
  currentTime = millis();

  //execute once per sample time-period
  if ((currentTime - timeBench) >= samplePeriod)
  {
    //reset timer
    timeBench = currentTime;

    //get data
    fabo_9axis.readAccelXYZ(&ax, &ay, &az);
    fabo_9axis.readGyroXYZ(&gx, &gy, &gz);

    //remove offset
    ax = ax - offsetax;
    ay = ay - offsetay;

    //scale by sensitivity
    scaled_gx = gx / gyroSensitivity;
    scaled_gy = gy / gyroSensitivity;

    //integrate gyro readings
    integrated_gx = scaled_gx * dt;
    integrated_gy = scaled_gy * dt;

    angleRollAcc = -atan2(ax, sqrt((ay * ay) + (az * az))) * 180 / M_PI;
    anglePitchAcc = atan2(ay, sqrt((ax * ax) + (az * az))) * 180 / M_PI;

    angleRoll = A_ * (angleRoll + integrated_gx) + (1 - A_) * (angleRollAcc);
    anglePitch = A_ * (anglePitch + integrated_gy) + (1 - A_) * (anglePitchAcc);

    //print for debugging////
    Serial.print("angleRoll: ");
    Serial.print(angleRoll);
    Serial.print(" anglePitch: ");
    Serial.print(anglePitch);
    Serial.println();
    /////////////////////////

    /*Serial.print("angleRoll: ");
      Serial.print((byte)map(angleRoll,-90,90,0,180));
      Serial.print(" anglePitch: ");
      Serial.println((byte)map(anglePitch,-90,90,180,0));*/

    horizontal.write(map(angleRoll, -90, 90, 0, 180));
    vertical.write(map(anglePitch, -90, 90, 0, 180));
  }
}
"The desire that guides me in all I do is the desire to harness the forces of nature to the service of mankind."
   - Nikola Tesla

Spange

Your problem is quite strange because gyros usually have drift (which is a low frequency noise) NOT high frequency noise like you mention. I think whatever noise you are seeing is probably negligible. What are you using it for? Can you provide test data/graphs?

Usually when I use an IMU such as the MPU6050, it is to calculate orientation Euler Angles with a complimentary filter. This uses BOTH gyro and accellerometer data simultaneously. Here is my code (for an MPU 9250):

Thank you!
My intent is just the same as yours, calculate the Euler Angles, Roll and Pitch.
I have not implemented that yet, thank you for your code. But when i have the MPU at rest on my desk I notice two things. A typical reading is [-0.082, 0.12, 1.012].

1) Even at rest the values fluctuate, are noisy, but this helped with a LP-filter. I could try to measure the amplitude of the noise if somebody is interested.

2) the norm og the measurement is not 1g. In other words: the measured gravitational vectors magnitude is not equal to 1g, at rest.

I did not quite understand you calibration script. To my understanding should not the calibration be a one time procedure to obtain some correction factors, and then implement those factor into the void loop of the Euler angle calculator?

How have you obtained your values:
offsetax, offsetay, gyroSensitivity ?

jremington

There is a great deal of material posted on line regarding calibration of navigational sensors. Please spend some time with google. For example, gyros exhibit drift and it is trivial to calculate the average drift and subtract. Unfortunately the drift depends on temperature.

For accelerometers and magnetometers see

https://edwardmallon.wordpress.com/2015/05/22/calibrating-any-compass-or-accelerometer-for-arduino/

Power_Broker

The code I posted was NOT a calibration program, but a Euler Angle calculator.
"The desire that guides me in all I do is the desire to harness the forces of nature to the service of mankind."
   - Nikola Tesla

Go Up