Gyroscopes, Accelerometers and the Complementary Filter

How Balance Works:-

In teaching a robot how to stand straight and not fall over, we need to understand how balance really works. Balance is something we take for granted in our daily lives, most of us are actually pretty good at it, until we have had a little too much to drink.

Here is how I think balance (writ easy) works:

1. We need a sensor that tells us how ‘upright’ we are standing. In humans, this sensor is located in the inner ear. In a robot, we could use a sensor like the MPU6050 which can be used to provide tilt in two axes just by directly reading from its registers.

MPU6050 from Invensense. 6 DOF sensor that can be used to determine attitude

MPU6050 from Invensense. 6 DOF sensor that can be used to determine attitude

2. Measure the deviation from the ‘upright’ position. Obviously as this deviation increases, it would eventually cause our centre of gravity to move outside the limits of our baseline, causing us to tip over.

If the COG lies outside the baseline, the body will tip over

If the COG lies outside the baseline, the body will tip over

3. To prevent tipping over… depending on the deviation observed, move the baseline to quickly get directly under the new position of the COG. In humans, while walking, we do this by moving our legs forward to arrest falling over forwards. In a wheeled balancing robot, we could measure the deviation from the ‘upright’ position and use motors to brings the wheels directly under this position.

So the first step is to extract angular deviation from the ‘upright’ position. Angular deviation in three axes is called ROLL, PITCH and YAW. To keep things simple, I have worked only with one axis, PITCH.

yaw-roll-pitch

The MPU6050 communicates to the Arduino through the I2C protocol. Here is the circuit I used to extract raw data from the sensor. I made this circuit using Fritzing.

Fritzing sketch of MPU6050 and Arduino UNO

Fritzing sketch of MPU6050 and Arduino UNO

The MPU6050 provides us with the following data:-

1. Acceleration measured in G forces, in three axes

2. Angular Velocity (rate of rotation) in deg/sec in three axes

The code for extracting raw data from the MPU 6050 has been written by “krodal” and can be downloaded from here. This code provides us data in RAW format (16 bit integer values) and we need to convert these into units we can use.

The MPU6050 works in various scales, details can be found in the datasheet, but in simple terms,

if the accelerometer scale is +/- 2G, then ANGLE IN DEG = raw value/ 182.

if the gyroscope scale is set to +/- 250 deg/sec then ROTATION RATE in DEG/SEC = raw value/ 131. We need to integrate this value over time to compute ANGLE from ROTATION RATE. This screen-shot shows data from all three axes graphed on my PC. The upper graph shows data from the accelerometers and the lower graph shows gyroscope data. I used Qt to program the graphing and my ArduinoTalker class to transfer data from the Arduino over the serial interface.

Unfiltered data (all three axes) from MPU6050. Top graph shows accelerometer data and bottom graph shows gyro data

Unfiltered data (all three axes) from MPU6050. Top graph shows accelerometer data and bottom graph shows gyro data

Now comes the tricky part, filtering the data to get the best value for PITCH.

(a) In the short term, the accelerometer output is terribly prone to noise. Therefore we need to use the accelerometer values only in the long term determination of PITCH.

(b) The Gyro values are much less noisy in the short term but will drift away from the true value of PITCH in the longer term.

So if we can apply a filter to (a) and (b), using the accelerometer readings in the long term and the gyro readings in the short term, we can get a steady, accurate value for the angle of PITCH. Such a filter is called a complementary filter, the formula is given below:-

The complementary filter

The complementary filter removes noise from the accelerometer and eliminates gyro drift

[EDIT 31 Jan 2014] I was prompted to re-examine my code by Ka, who commented on this article and brought up a very interesting point. In short, and because of the way accelerometers work, the data I have used to run this filter came from two mutually perpendicular axes. Look at the code below to see what I mean…..the details are in my reply to Ka on 31 Jan 2014 (to whom I am very grateful for pointing this out to me).


float a = accel_t_gyro.value.y_gyro/131;
float b = accel_t_gyro.value.x_accel/182;

Here is an image of the filter at work on my PC:

MPU 6050 data manipulation

MPU 6050 data manipulation. Time on the X-axis.

1. Then green lines shows gyro turn rate in deg/sec. These values are integrated over time to produce the yellow line.

2. The noisy blue line is data from the accelerometer

3. The smooth maroon line is data that has been filtered using the complementary filter given above.

As can be seen, the filtered line is free from short term noise and does not drift like the yellow line in the graph.

If our robot is tipping over, we can now use this data to take some sort of corrective action by turning motors etc. In this example I have used data from just one axis – PITCH. We can also use PITCH to create forward/ backward motion. This is what devices like the SEGWAY use, leaning forward on the pedestal causes a permanent PITCH angle that the machine tries to eliminate by moving its motors resulting in constant forward motion.

To determine both ROLL and PITCH, we need to manipulate data from two axes, which is fairly straightforward and can be dome using the filter given above. Three axes attitude estimation is quite a different story, involving complications like the Gimbal Lock and Quaternions, and while it can be done internally by the MPU6050’s Digital Motion Processor, I haven’t yet figured out how to extract and interpret this data.

Advertisements

57 thoughts on “Gyroscopes, Accelerometers and the Complementary Filter

  1. Pingback: Balance Good, Everything Good | Bayesian Adventures

  2. Hi there thanks in advance for these great information.When I run this sample code that you mentioned in link,I dont get any error but I cant see anything on serial monitor even any words.What do you think about this problem? I really need to solve this to continue my project .

    • Are you talking about ‘krodal’s’ code from the arduino.cc website? Off the top of my head I would think maybe its an I2C problem. Look at this part of the MPU-6050 code…..

      // Default I2C address for the MPU-6050 is 0x68.
      // But only if the AD0 pin is low.
      // Some sensor boards have AD0 high, and the
      // I2C address thus becomes 0x69.
      #define MPU6050_I2C_ADDRESS 0x68

      Is your AD0 pin high or low?

      The connections between the Arduino and the MPU should be as follows…

      MPU Arduino

      Vcc +3.3V
      GND GND
      SDA +3.3V
      SDA Analog 4 (UNO)
      SCL +3.3V
      SCL Analog 5 (UNO)
      AD0 GND (for address 0x68)
      OR maybe AD0 +3.3V (for address 0x69)

      I think this is the most common type of problem.

      • Hi yes I am talking about krodals code.I am sure that I made connections correct but I am trying to use these codes in Energia for msp430.In energia ardunio codes works for msp430 too.Maybe thats why although I dont get any error,I cant see anything in serial monitor.I tried both 0x68 and 0x69 for I2C adress.

      • By the way,When I give power to MPU6050,red led lights on the mpu6050 board.On the net I usually saw green light when it is opened.It sounds stupid but I wanted to ask it too 🙂

  3. I have to say this is amazing! and I have a question about the filter, you had stated that the output of the data is in terms of angles, and it matches the Acceleration data very well. does the data for the acceleration come from linear movement of the accelerometer? or does it come from the rotation of the accelerometer? would it be possible to use something similar to track and smooth out accelerometer data? or would I have to use a Kalman filter?

    • Thats a very interesting question.

      I guess the answer is that the output of an accelerometer is a combination of motion along each of its axes and the force of gravity sensed along each of its axes.

      When the sensor is stationary, the output would only be dependent on gravity and as it is tilted, the force of gravity sensed (on the two axes perpendicular to the tilt axis) would change.

      I looked at my code again and found that this is exactly what I was reading from the MPU6050. Data from two different axes to determine PITCH angle.

      float a = accel_t_gyro.value.y_gyro/131;
      float b = accel_t_gyro.value.x_accel/182;

      As you can see from the code above, the data to my filter came from two mutually perpendicular axes. I think I completely forgot to mention that in the blog post.

      To smooth out accelerometer data I recommend a simple moving average filter. I dont know too much about kalman filters except that they are terribly complicated to implement. As I understand, kalman fliters also need an “ACTION” parameter in order to work properly. Truth be told, I know very little about them, but a simple moving average filter should work just fine. The other option is a recursive bayesian filter which is much easier to implement. Theres a great tutorial by studentdave on youtube about this filter.

      Thanks so much for reading and helping me identify that little detail about the accelrometer axes I seem to have missed out. I will edit my text at once.

      • Hi:

        I just happen to work on MPU6050 with Arduino Uno R3 and would very much like to see the 2D display of the MPU6050 output.
        This seems to be the exact what you have done last year !
        I have the MPU6050 with Arduino working now and can see the live data from MPU6050 my laptop.
        I now want to do what you have done – using Qt to display the waveforms with the filter too.
        But I am new to Qt and not sure where/how to start.
        Can you tell me (or point me in the right direction) the codes you used and the associated instructions to get the Qt work started ?
        BTW, I prefer using Mac if that makes any difference for Qt.

        Thanks in advance.

        Dan

      • Thank you, I’ve tried a simple moving average filter, but the data is a little too inconsistent at times. The Kalman filter is very complicated to work with, I’ve tried as well and was unable to really figure it out as well as I had hoped. I’m currently trying to track the object’s linear movement. It seems that the tracking that you’re doing based on the force of gravity comes more from tilt then the linear movement, but it does help later on. Thank you once again!

  4. Hi:

    I just happen to work on MPU6050 with Arduino Uno R3 and would very much like to see the 2D display of the MPU6050 output.
    This seems to be the exact what you have done last year !
    I have the MPU6050 with Arduino working now and can see the live data from MPU6050 my laptop.
    I now want to do what you have done – using Qt to display the waveforms with the filter too.
    But I am new to Qt and not sure where/how to start.
    Can you tell me (or point me in the right direction) the codes you used and the associated instructions to get the Qt work started ?
    BTW, I prefer using Mac if that makes any difference for Qt.

    Thanks in advance.

    Dan

      • Thanks.
        I have another window based laptop and have downloaded there.
        My s/w skill is not very good. something very obvious to experienced people can be a challenging thing to me. Going through this seems takes much more time than anticipated.
        Seriously, if you think this is not a big task for experience person, would you mind to guide me through this by phone ?
        I will buy you a gift card or something similar.
        I need the help and I am serious.
        I have the h/w portion working already (MPU6050 with Arduino Uno R3.
        Dan

      • Sorry, circumstances here do not permit me to talk over the phone. If you are a c++ or a Qt beginner, this project may be a little too difficult. I strongly recommend learning a little Qt first. Brian (voidrealms) has uploaded some excellent tutorials on youtube. Thats where I started….it only took me a couple of weeks.

      • OK. Thanks.
        I may not have a few weeks to learn all this.
        I would really appreciate if you know anyone who can help.
        If phone call does not work, perhaps this can be worked out over the internet….using screen sharing….

      • i would like to help if i can….

        where exactly are you having problems understanding the code?

        are you getting compiler errors?

      • Thanks for the response.
        I now have it wiring using Processing.
        I can see the waves and I am fine for now.
        Thanks again.

  5. OK. Thanks.
    I may not have a few weeks to learn all this.
    I would really appreciate if you know anyone who can help.
    If phone call does not work, perhaps this can be worked out over the internet….using screen sharing….

  6. Pingback: Removing Noise : The Versatile Complementary Filter | Bayesian Adventures

  7. Pingback: Still working on the Autopilot | m/v C:[ESC]

  8. is this available on windows?
    i am trying to get my own school project working and i can get any code for graphing out the values i get from krodal’s code.
    i only know basic java but c++/processing seem very alike so i get most of it but i have no idea how to get a graph.
    any link i see try on this just get me to a download website where i get a zip file i can unpack and then not use for anything. i have no idea what to do after i unpack it, or if i just cant unpack it the right way(left click unpack?). i get 2 file i cant open or use for anything and feel like ther has to be a easy way to solve that.

    • http://www.filefactory.com/file/6i4xdwogd2f/ComplementaryFIlter_MPU6050.zip

      This code will compile correctly under the Qt 5.0 IDE. If you are using Java, you will need to find some other way to …

      1. open the serial port and stream data from the Arduino to your PC
      2. Parse this data stream into numbers that represent acceleration/ tilt
      3. Use a Java graphing class to plot these on your screen.

      I don’t know much about Java and Windows but I think that if you’re not too interested in a fully functional GUI for your program, the easiest answer may be to just use Processing.

      • thx i think i got it to work a bit now.
        however i seem to get a error i know nothing about “:error: No rule to make target ‘../Classes/plotter.cpp’, needed by ‘debug/plotter.o’. Stop.”
        its the second error i get but the first was just changing the include files to my own address.

        also i have no idea how to use Qt, i the arduino programmer where i have the code for retrieving the raw values, and i have no idea how to get that into Qt or reverse if thats possible

      • what is the compiler you are using? plotter.cpp and plotter.h are the files needed for the plotter class that is used to plot the graph. This plotter class is dependent on several Qt classes and properties and the program will not compile unless you are using Qt.

      • no idea,
        like i said i have no idea what i am doing in Qt. thanks for your but i got some help to graph it out in processing which kinda work, and the school project goes on so am writing the report now and if i get time i will start working on the noise filtering.

  9. Hi dude,

    Thanks for the topic. That’s a great sharing. I need to get the data from mpu6050 to orientate my quadrotor. I read many articles about it and I’m a little bit confused. If you help me about the questions below, I will appreciate

    To find ‘gyroRate’ , we will use this formula ” gyroRate = (gyroAdc-gyroZero)/sensitivity ”

    Is this ‘sensitivity’ value for my choice?

    How can I find the ‘gyroZero’ value?

    To find ‘accVal’ we will use this formula ” accVal = accAdc-accZero ”

    How can I find zero voltage and also accZero? Is not sensitivity needed to calculate acc value?

    Thank you

    • I’m using the accelerometer readings to give me a value of static angle, not to calculate linear movement. For example, if accelerometer Z is showing me a value of 1, when I tilt the MPU6050 by 45 degrees, it should now show me 0.5.

      So, 16384 LSBs actually correspond to +/- 90 degrees of tilt. and so to extract the static angle of tilt from the raw accelerometer value, 16384/90 = 181 (approx)

      • Yes dude,

        I checked it but I’m using STM32F4 and there is no library for it and I can’t implement the code ,which you gave, into STM. That’s why, I need to get yaw value without DMP

  10. Pingback: Self-Balancing Robot -Use Arduino for Projects

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s