How to smooth out gyro data

I’m using MPU6050 on the Mega 2560 and the data I get when transferred directly to Processing give me a very shaky and unstable movement of the 3d box. I have no “filters” in the code. How do I fix this?

Arduino Code:

const int MPU=0x68; // I2C address of the MPU-6050
int AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
void setup(){
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
void loop(){
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.requestFrom(MPU,14,true); // request a total of 14 registers<<8|; // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)<<8|; // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)<<8|; // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)<<8|; // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)<<8|; // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)<<8|; // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)<<8|; // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
Serial.print("AcX = “); Serial.print(AcX);
Serial.print(” | AcY = “); Serial.print(AcY);
Serial.print(” | AcZ = “); Serial.print(AcZ);
Serial.print(” | Tmp = “); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet
Serial.print(” | GyX = “); Serial.print(GyX);
Serial.print(” | GyY = “); Serial.print(GyY);
Serial.print(” | GyZ = "); Serial.println(GyZ);

Hi Martin,

the most simple way to setup a filter would be as follows:

Take n raw samples and just average them. n is a number that you have to define for your individual problem. The higher n, the smoother the result, but the lower the final framerate.

You could also feed your raw samples into a first in first out-register of length n and average over the content every cycle.

There are also many filter-libraries out there like this one:

or just google arduino noise filtering.

Signal filtering can be quite complicated, depending on the noise-spectrum of your signal source but also very interestning!

Good Luck.

Thanks for the quick and great response! I tried a bunch of different types of filtering and what I found to be the most helpful was simply to filter out every change less than 200 bits. It's not the best solution at all, but does the job at the moment. Will look into the library above though!

Hmm what do you mean by LESS then 200 bits? If you suffer from sudden spikes in a otherwhise well constituted signal, there could be also a programming/interpretation problem. Is the temperature signal okay?



The temperature is fine and stable. The others are pretty much kind unstable. Since the values go from -32k to +32k bits the idea is that 200 out of a range of 64k values won't do harm. It just says that if the signal is changing less than +-200 do not print it. Otherwise, do print.

Ah! Now I got it!