Arduino IMU: Pitch & Roll from an Accelerometer

One day, looking for cheap sensors on ebay, I found this interesting board which contained everything I was looking for. It basically consists of a 3-axis accelerometer (ADXL345), a 3-axis magnetometer (HMC5883L), a 3-axis gyroscope (L3G4200D) and a barometric pressure sensor (BMP085). My plan is to build an Inertial Measurement Unit (IMU) (or maybe I should call it Attitude and heading reference system (AHRS)) and in the process learn how to interact and interpret the information all of this sensors provide. The fact is I have some experience using IMUs since I used one on my master thesis and another one on the Avora AUV, but the fact is they come preprogrammed and there is not much point in working with the raw sensor data unless you want to improve the measurement or give it another use.

For this project I am also using an Arduino Duemilanove, for that reason I wanted to call it ArduIMU, but there is already another project with the same name, so I will have to find another name (suggestions would be appreciated). Connecting the sensor board to the Arduino is pretty straightforward, every sensor has an I²C interface so you can access each of them using the Arduino Wire Library. The drawing was done using fritzing, on which I created the corresponding custom part for this board, although I did something wrong and it does not conform to the fritzing graphic standards.

This will be the first of a series of posts I plan to write about this project, since there are several steps I need to take in order to fully understand each sensor and several more to combine them in order to improve accuracy. In this post I want to talk about the accelerometer and how to obtain the roll and pitch angles from it, which is a process that can also be called tilt sensing.

Accelerometers are devices that are capable of measuring the acceleration they experience relative to free-fall,  the same acceleration living beings feel. As a consequence, accelerometers are incapable of measuring the acceleration of gravity, but can be used to measure the upwards acceleration that counters gravity when at rest. This acceleration is measured as 1g on the z-axis, when both pitch and roll angles are zero, but when the sensor is tilted either the x-axis or the y-axis experiences a component of the upward acceleration, whose magnitude depends on the tilt angle.

Pitch & Roll estimation

Obtaining the pitch and roll angles is then a matter of being able to read the accelerometer, convert these readings to the g unit (1g = 9.8 m/s²), and apply the corresponding equations. The process of obtaining and converting the accelerometer readings depends on the accelerometer you are using, in my case, the ADXL345 in its basic configuration, provides 10-bit resolution for ±2g, but has several other ranges (±2g, ±4g, ±8g, ±16g)  and resolutions (from 10 to 13 bits depending on the range) . Generalizing, the formula used to calculate the acceleration from the accelerometer readings is:

G_{Accel} = Raw_{Accel} \cdot \dfrac{Range}{2^{Resolution - 1}}

Once we have the correct acceleration components, we can proceed to calculate the different angles using the following equations:

pitch = \arctan{\left(\dfrac{G_y}{\sqrt{G_{x}^2 + G_{z}^2}}\right)}     roll =\arctan{\left( \dfrac{-G_x}{ G_{z}}\right)}

For more information about where these equations come from, you can read the documentation I include at the end of this post. As you can see, the denominator of the pitch equation is defined to be always positive, so the equation itself only provides [-90, 90] range, which is exactly what is expected for the pitch angle. In contrast, the roll equation provides [-180, 180] range. It is important to take into account that when the pitch angle is 90º, the surge axis (roll) is directly aligned with the gravity vector, thus we cannot measure the roll angle anymore, this is what is called Gimbal Lock.

Also, be aware that the roll equation is undefined when both G_x and G_z are equal to zero, and that for each possible value of the calculation done inside the arctan function there are two valid solutions, not only on the roll but also on the pitch equation. These problems can be easily solved in code by using the function atan2, which eliminates the angle calculation ambiguity by taking into account the quadrant.

Removing short-term fluctuations using a Low-Pass filter

At this point we already have a fully functional pitch & roll estimation system, but if we experiment with it we will discover that the readings fluctuate quite a bit and this may be very annoying for some applications. Removing these short-term fluctuations can be achieved by means of what is called a Low-Pass filter. This type of filter attenuates the higher frequencies of the signal, thus providing a smoother reading. The Low-Pass filter is easily implemented by using the following equation:

y_{t} = \alpha \cdot x_{t} + (1 - \alpha) \cdot y_{t - 1}

Where y_t is our filtered signal, y_{t-1} the previous filtered signal, x_t the accelerometer reading and \alpha the smoothing factor. It probably may seem obvious, but filtering should be done to the accelerometer readings before calculating the angles, instead of to the angles themselves. Regarding the smoothing factor, the lower we set it, the more it will take for the angle to stabilize, so we should not set it too low because then we could lose real-time behaviour. With this I mean that the reading will not correspond to the real angle until it stabilizes, and this could take some time.

The source code & the ADXL345 library

I developed a small library to interface with the accelerometer, even though at the moment I have only implemented the basic functionality, I plan on supporting all of the device features. You can find it in my github account, where you can also find the processing code I used for the video example below. Thanks to the library, the code is pretty straightforward. It just reads the sensor accelerations which are already converted into gs by the library, applies the Low-Pass filter and then uses the roll and pitch equations to calculate the angles.

#include <Wire.h>
#include <ADXL345.h>

const float alpha = 0.5;

double fXg = 0;
double fYg = 0;
double fZg = 0;

ADXL345 acc;

void setup()
{
	acc.begin();
	Serial.begin(9600);
	delay(100);
}

void loop()
{
	double pitch, roll, Xg, Yg, Zg;
	acc.read(&Xg, &Yg, &Zg);

	//Low Pass Filter
	fXg = Xg * alpha + (fXg * (1.0 - alpha));
	fYg = Yg * alpha + (fYg * (1.0 - alpha));
	fZg = Zg * alpha + (fZg * (1.0 - alpha));

	//Roll & Pitch Equations
	roll  = (atan2(-fYg, fZg)*180.0)/M_PI;
	pitch = (atan2(fXg, sqrt(fYg*fYg + fZg*fZg))*180.0)/M_PI;

	Serial.print(pitch);
	Serial.print(":");
	Serial.println(roll);

	delay(10);
}

The result

For a more interactive visualization of the data, I also developed an example using processing, which consists on a rotating 3D cube. You can see the results in the following video.

In the next post about my Arduino IMU, I will talk about how gyroscopes work and how to interpret the information they provide.

62 Comments

  1. Very clear presentation! Just a comment that there is a noticeable delay between you moving the accelerometer and the cube image moving. This could significantly detract from the agility of a robot or game player using this system. Considering that the mechanics processing required is negligible, this leaves the sensor latency, smoothing filter and graphics processor as possible sources of delay. Could this lag be reduced by assuming continuity using something like Taylor series prediction?

    1. I would have to look into it, but it probably can be done if you assume continuity. For most applications I can think of, it wouldn’t be very useful though.

  2. hey buddy.. here’s my contribution, how about calling it imuino, you could use the emu bird as a logo or am I going too far with it?

  3. Hi , i have the exact same sensor (you can buy it for 20bucks on dx.com) , but i didnt start to work on it yet , but your tutorial will be really useful, thank you .
    i have a question thou ,
    can you get SWAY SURGE and HEAVE “like” data , by subtracting the angle from the gyro to data of the accelerometer , and use the result to get translation (or more like the position relative to the starting point), with doing something like x+=x; y+=y; z+=z; (in the loop) with the result ?
    ? (not sure if i am clear , i am a beginner).
    you could use something with DOF in your lib name (Degree Of Freedom) .
    Cheers

    1. Thanks! Well, the gyro doesn’t really give you an angle but an angular rate. Giving that you don’t have translational speed, nor is it calculated anywhere, you can’t really measure translation. What you are asking falls more within the grounds of an Inertial Navigation System (INS), and in itself is very a complicated problem, but an inaccurate approximation could be done by integrating over time the acceleration.

      1. i am getting confused , (it is quite out of my league but it is so fascinating!)
        the accelerometer gives the acceleration (relative to gravity) (if not translating , gives roll and pitch)
        the gyro gives the angular rate (=angular velocity=speed of rotation ; different from the acceleration of the rotation ?) (if integrated gives the orientation ?)
        but then the magnetometer (understand compass) gives the orientation ,right ?

        so all the youtube videos of people doing the Euler thing (orientation tracking) with 6DOF sensor , are basically doing the work of the magnetometer ?

        if i want to do the integration over time (extract translation and find position) (solving the shifting with a regular manual reset/recalibration) , i would need to remove the pitch yaw roll from the accelerometer data resulting only in the translation acceleration , right ?

        also this is relevant http://www.youtube.com/watch?v=6ijArKE8vKU

        do you want we continue this “talk” on a forum ? i don t want to “spam” your comment space.

        1. First of all, the accelerometer gives yo the acceleration relative to free-fall, so can actually extract a “gravity” vector from it when at rest, that may have a component on the x, y and z axis. Since the gravity vector es pointing up because it’s not really gravity but the acceleration that counters it, you can only extract tilt from it, i.e. pitch and roll.

          The gyroscope gives you the angular rate, which I believe it’s equivalent to angular velocity, certainly not to acceleration. So if you integrate over time you can obtain the orientation, but not quite, it’s something I’ll talk on the next post about the Arduino IMU.

          Magnetometers measure magnetic field in each axis, so you can “sense” the earths magnetic field and calculate the orientation relative to the magnetic north (yaw), but there are techniques to calculate pitch and roll also. I will talk about it in future posts.

          I don’t really know about the videos you’re talking about, but calculating precise orientation usually requires a combination of sensors.

          Regarding the translation measurement, you can extract the gravity vector from the accelerometer readings, so all you have left is the acceleration you need for calculating the translation. But you must remember that integrating over time accumulates a noticeable error and, in this case, you must integrate twice (acceleration->speed, speed->translation).

  4. There is a gyro on that board too, mix it with the accelerometer. Combine the outputs with a complementary filter first. Fool with the crossover points while graphing the outputs, adjust until you get an acceptable error from the fastest response you want to perform in. After that, you can start playing with extended kalman filters, they do a great job of mixing the best way possible, but they might run a little slow.

    Nice job.

  5. Hi everybody, could you please tell me about formula of yaw estimation ?
    I mean that, there is written that to get roll from adxl345 is arctan(-Gx/Gz), so what about yaw ?
    I hope understand later, thanks.

  6. Hi, i have a problem with my sensor, it seems that it only shows one value for pitch and for roll..

    This is my output in the Arduino Serial
    -34.09:135.67

    I only get to print something when SDA and SCL is disconnected, when I connect the wires, the serial stops.

    Any idea why? Is my gyro damaged?

    I am using an Arduino Mega

        1. I am using the library that you posted here. All I want is to make it work and display the pitch and roll.. it is connected to 5V-GND and the SDA and SCL to pins 20/21 in the Arduino MEGA

        2. I’m having the exact same problem, except that I’m using an Arduino UNO instead. Did you make it work? Great article, BTW!

  7. Nice post! Really liked it! Helped me a lot. Congrats!
    Just one thing: isnt the Low-Pass filter equation with a sign error? Shouldnt it be alpha*x + (1-alpha)*y ?

  8. Hi found this very interesting I’m going to convert this to python and give it a go in the next couple of days it actually looks pretty simple. Any chance of getting the next part up on the gyro?

  9. Yes, I will! I’ve just had two pretty busy months and haven’t had much time. Thanks for following and please be patient.

      1. Thought i’d post a quick message I implemented your code in python on my Raspberry Pi and am seeing very good results. I do get some odd noise on occasion which will give me a 2g spike (in a +-4g range) on the Z and X axis but never Y! Not sure why. I’m currently writing out to a txt file and then importing the results into excel for graphing. Now i’m happy my code is working ok i’m going to start creating a real time GUI. Not bad for my first bit of programming.

        1. I’m happy it worked, thanks for writing! I hope in less than a month I’ll be able to write the next step on this project. I’m actually two/three steps ahead, but I don’t find the time!

  10. Great job and a great post. Did you ever post your source code for the cube visualization? I’m very interested in doing something similar, so any guidance would be appreciated.

  11. This is the perfect site for anyone who would like to understand
    this topic. You understand so much its almost tough to argue with you (not that I actually
    will need to…HaHa). You definitely put a brand new spin on a
    topic which has been written about for years. Wonderful stuff, just great!

  12. You have observed very interesting points !
    ps nice site. “Formal education will make you a living self-education will make you a fortune.” by Jim Rohn.

  13. Hi, Nice project, I have the exact same sensor and want to start playing with it.
    I have been to your github and downloaded the zip, I renamed the arduino folder as suggested, opened it in arduino , and it wont compile,
    I get the following message :
    “pitch_roll:20: error: ‘ADXL345’ does not name a type
    pitch_roll.cpp: In function ‘void setup()’:
    pitch_roll:24: error: ‘acc’ was not declared in this scope
    pitch_roll.cpp: In function ‘void loop()’:
    pitch_roll:33: error: ‘acc’ was not declared in this scope

    do you know what it going wrong ? from my knolwege of arduino the ACC function is not being recognised, but I have the folder correctly named with all files inside it and in the library folder,

    thanks for any help , I look forward to playing with this

    1. You need to put the renamed arduino directory in your workspace under another directory called libraries. If this directory doesn’t exist you should create it. Then open the example and try again. Cheers.

      1. hey ,
        I got it working last night, turns out there is a bug with arduino and you need to open the file in a certain way or it wont compile,
        did you ever integrate the gyro into it ? I have tried the multiwii code with this IMU and its a lot smoother , although not as easy to get the attitude values out of,

        so far im happy,:)

        1. sorry , i meant to say thanks too, good work ,
          do we know what angle presicion this gets?

  14. Hi, you are using +-2g range on the accelerometer right? So, you should get +-512 radings no? That is what i’m getting but the pitch and roll angles have really bad readings like 12453 or even 23875.
    What is the problem?
    Please help me!

    1. I was able to resolve the problem…I don’t know what it was, I just started from the beginning and it worked from the second try. Thanks very much for your project it was of great help.

  15. Hi. I am working one week for How can I calculate yaw and use gyro ? I can find roll and pitch but not find yaw. I work with ROS and rviz only accept quaternion. IMU is MPU6050 6DOF. Some people say degree = angular celocity * sample rate. But I try a lot of way but I do not do. How can I use gyro and find yaw ? Thank you good work.

  16. Hi,
    I’m working with IMU.
    I need to define, every istant, the rotation matrix to do a coordinates changes from sensor coordinates system to a fixed system defined by the initial sensor orientation.
    I have also the gyroscope data. But knowing the pitch, roll and yaw angles there are 6 rotation matrix build changing the order of the three rotation. How can I solve this problem?

    Thanks

  17. Good afternoon.
    I am a student in Korea. For not proficient in English Thank you for your understanding.
    Google translator was used.

    Well I saw a great movie!

    Time of graduation, work with Unity & Arduino is trying to create work.
    Accelerometer use here but I can not think of what to do to try. not gyro..
    It was found during a Google search you gonna be okay?
    MMA8452Q Accelerometer sensor was used.

    MMA8452Q XYZ values ​​received by vector3 (forward, left, rifht) will try to change.

    Please respond. Thank you.

  18. Hi!

    I’m so excited that i found this post! i’m a beginner interested in developing navigation/control of rov/auv. Please can you guide me how to proceed after i have an imu that gives magnetic heading, pitch and roll. Is it necessary to have mathematical model for rov/auv? how do i determine the model? Please can you also teach me whether kalman filter is necessary or not, if yes then how is it implemented, what are the steps, inputs and outputs? i hope you can help

  19. Hi ! I’m using a seeeduino Mega 2560 + GY-87 module.I’ve attached the moudele to the 2560 board (GND to GND, VCC_in to +5V, SCL to pin ANALOG 5 and SCA to pin ANALOG 4) and I’ve compiled the code but when i opne the monitor the values don’t change, I get the same value all the time….

    Any ideas?

    Thanks in advance for your help.

  20. It is interesting to know accuracy level of the angle measurement achieved using this set up?

    Is it only static measurements. Hopefully good enough for slow varying angle(less than 20 cycles per sec.) measurements.

  21. Hi Anil,
    This blog was of great help, thank you. You did talk about your next blog about gyro and its intergration with acc where can i find it ? I am facing some issues when i intergrate acc and gyro. It would be of great help if i could get some inputs from you about gyro. Thanks in advance.
    Regards,
    Shweta

  22. Very good for a newer, thanks very much!
    But could you explain the formula from arctan to atan2 more detail ?

  23. Hello

    thank you for the nice effort

    how can we make visualization like what you did

    tell us about that pleas

    thank you again

  24. Can I ask for the gyroscope Fritzing file? Currently I’m making a project with the same gyroscope and I need that fritzing drawing

  25. Hi,.,
    I am working on an Arduino based IMU module which will be used to determine human joint angles. I was going through articles and came across your tutorial page and found it extremely helpful to understand the basics. Thank you!!

    The Inertial sensor I am using is the GY-80 module (I am using only accelerometer ADXL345 and Gyroscope L3G4200D)

    When comparing the Accelerometer angle data with that of the gyroscope, I find that they are not really similiar. (The code I am running is based on several codes found online).

    For the accelerometer angles I have used the formulae:

    angle_x= (atan2(xg, sqrt(yg*yg + zg*zg))*57.2957795);
    angle_y= (atan2(yg, sqrt(xg*xg + zg*zg))*57.2957795);
    angle_z= (atan2(sqrt(xg*xg + yg*yg), zg)*57.2957795);

    And for angles using gyroscope, I have integrated them using the trapezoid method (I hope I am calling it right!!!)

    I placed a micros() timer at the end of the setup loop and have reset the timer at the end of the angle computation part.

    angle_gx += gx*((double)(micros()-time1)/1000000);

    angle_gy += gy*((double)(micros()-time1)/1000000);

    angle_gz += gz*((double)(micros()-time1)/1000000);

    I have spent a lot of time in this and I am not able to proceed further😦

    I would be highly grateful if someone could help me with it!!! Are the formulae mentioned above correct???

    Please let me know if I should copy the code and results as well!1

    Thank you

  26. Hello,

    I was looking into your formulas and the code that you implemented. On the formula for pitch the numerator is Gy while on the code you are using fXg and this isn’t the only deviation from the formulas since there are many.

    Which one is correct? The formula or the code?

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