My first 6 DOF IMU Sensors Fusion Implementation: ADXL345, ITG3200, Arduino and Processing

Last updated on Mon, 2012-07-23 09:09. Originally submitted by fabio on 2010-11-22 21:36.

When you have created two breakout boards for the ADXL345 accelerometer and the ITG3200 gyroscope and you have those two nice sensors what you can do? Simple: create an implementation of an Attitude sensor fusion which runs with those chips!

So, I started documenting myself and reading lot of stuff on IMUs (inertial measurement units - composed by an accelerometer and a gyroscope) and MARG sensor (an IMU which also has a magnetometer to sense heading).

One really good article you should read to get started with IMUs is A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications available on . Also from the same website there is a nice implementation of the theory from the IMU guide: you can find it on Arduino code for simplified Kalman filter. Using a 5DOF IMU.

So, I took the algorithm above and converted it to be used with the ADXL345 and the ITG3200. I came out with two implementations both with parts in Arduino and Processing code (see the attachments to this page below).

IMPORTANT: If you use the IMU Digital Combo Board from Sparkfun you will need to change the address of the Gyroscope from 0x69 in my code to 0x68.

The first implementation simply reads the raw accelerometer and gyroscope values on the Arduino while the Processing code (running on the PC) will compute the sensor fusion algorithm, produce the orientation vector and siplay a nicely oriented cube.

In other implementation all the sensor fusion logic is implemented in Arduino code so it will run embedded in the microcontroller. Looks like Arduino can coupe with that without any problems! So, once the orientation is computed is all sent to the PC where the Processing application will display incoming data and the oriented cube.

You can see a demonstration in the following video:

All the code is available in the attachments below. You can see the circuit in the picture below. Note that these are custom made breakout boards, your pin configurations will vary depending on the schematics of your breakout board. If you have questions just leave a comment below.

The processing programs need to be executed on the Processing IDE, available for download from
Remember to adjust the serial port configuration in the Processing code to match your connection to the Arduino in your system. Here I use /dev/ttyUSB9, if you are under windows you'll probably have to use something like COM3 or something like that. See in the Arduino IDE under Tools->Serial Port to get the exact value you have to use.


Arduino Code IMU 6DOF - ADXL345 ITG3200 on chip filter15.03 KB
Arduino Code IMU 6DOF ADXL345 ITG320013.64 KB
Processing Code 37.56 KB
Processing code for on chip version38.77 KB

an observation

Submitted by Mike Q (not verified) on Sun, 2011-04-24 04:32.

it seems that no matter how i move the accelerometer the max value read is 1g.

does this program only read up to 1g or does it only display up to 1g?

Hey Mike, thanks for stopping

Submitted by fabio on Tue, 2011-04-26 00:07.

Hey Mike, thanks for stopping by.

The accelerometer readings get normalized so that the sum of the XYZ readings always gives 1. It actually reads any kind of acceleration read from the accelerometer, even greater than 1g, but then it normalize to sum to 1 and use that as input to the sensor fusion algorithm.

Hope this helps,



Submitted by Don (not verified) on Wed, 2011-04-20 03:01.

Extremely helpful site! I have read through all comments and answers (multiple times!), downloaded your code and I am up and running with my Sparkfun Sensor 10121. I am using the Arduino Uno and need to have the chip perform the calculations so I am using that version of your code. My project involves using the angle data to control two stepper motors. Do you have any simlar projects, or could you point me in the right direction? I am new to the Arduino and C type programming. My Engineering career of over 35 years has been in structures, not electronics.
I looked at a LOT of sites and formum, and yours was by far the most helpful. I applaud you for sharing your expertice. Hope you have a productive day.

Well, I don't have any

Submitted by fabio on Wed, 2011-04-20 06:28.

Well, I don't have any personal experience in controlling motors from an IMU, however I think you can find something good searching about Arduino controlled 2 wheels self balancing robots, like this one.

To me, controlling the two motors of the wheels of a self balancing robot looks like a similar problem as what you have.

If this approach proves useful to you, I can also get you in touch with the guy behind this other self balancing robot, which uses the same sensors of this article so it may be directly used by yourself. Just ask me in case.

Hope this helps,


Thanks for the direction! I

Submitted by Don (not verified) on Wed, 2011-04-20 13:35.

Thanks for the direction! I will check out both sites and see what they have done. Once again, I really appreciate you and your site. Keep up the good work.

Timing Questions

Submitted by Mehdi (not verified) on Tue, 2011-04-19 02:43.

Hi Fabio,

Thanks for posting this code, as it was very helpful for many people, including myself.

I have a question regarding the time it takes to read the ITG3200 and ADXL345. When I say "read", I am referring to a bulk read of the 12 data bytes for each sensor. I did a test on the oscilloscope, and found that it takes about 180 usec to do a bulk read on each sensor -- so 180 usec for the gyro and 180 usec for the accelerometer. I believe I am using fast I2C mode. Does this sound about right to you?

The reason I care about this has to do with my sampling. Let's say for example that I choose to sample the ADXL345 @ 400 Hz and the ITG-3200 @ 500 Hz (unfortunately, unless I read the data sheet incorrectly, I can't get them both to 500 Hz). The microcontroller itself must sample these sensors via I2C every "X" Hz. Because of other things I am doing, I am having trouble making "X" 1 KHz. I initially thought I needed 1 KHz to get twice the sampling frequency of the ITG-3200 -- what I thought to be fulfilling the Nyquist rate. However, I believe I made a mistake. The Nyquist rate has to do with the maximum frequency of the signal -- in this case the bandwidth. Is this correct?

If this is true, then I believe I can down my sampling to 500 Hz without violating any sampling principle -- as my bandwidth is well below 250 Hz. There is however, the issue of synchronizing the microcontroller clock with the sensor timing. Nonetheless, I don't believe this should be a huge deal -- even if the clocks are not synchronized, the microcontroller should still pick up a new sample every 500 Hz.

I realize my logic may be wrong somewhere above. If so, please let me know where.



I think that your reasoning

Submitted by fabio on Tue, 2011-04-19 20:58.

I think that your reasoning sounds good.. what is your project?

Serial Logger

Submitted by Mehdi (not verified) on Thu, 2011-04-21 18:24.

Hey Fabio,

Thanks for your reply. I am making a serial data logger to record 6dof movements over several hours at a time.

I am running into a problem regarding the bandwidth. I placed the sensor on a mechanical rotator and to test the sensor at different sinusoidal frequencies and peak velocities. I use a computer and DAQ to control the motor. At frequencies of 2 Hz and below, things look pretty good. At 5Hz, I get some small cutoff. However by 10 Hz, I get some pretty big cutoff, something like -4.5 dB.

This is not what I would expect, as I set the cutoff frequency to 20 Hz, or so I think. I set register 0x16 (DLPF) to 0x1C. I believe this corresponds to +/- 2000 deg/sec (FS_SEL = 3) and 20 Hz LPF bandwidth (DLPF_CFG = 4). The sample rate register (0x15) is set to 0x01, which I believe would set the gyro sampling rate to 500 Hz. In my microcontroller, I am reading the I2C every 512 Hz.

Do you see where I am possibly making a mistake? I trust that the rotator is doing what it is supposed to, as I have tested it for other things in the past. I also trust my serial communication. Since I get proper data at lower frequencies, I believe it is either a problem at the microcontroller or ITG-3200 level (such as improperly setting the registers). I would appreciate any insight you can provide. I can export pictures of the data I'm getting if need be.




Submitted by fabio on Fri, 2011-04-22 10:23.

mmm... not many ideas about this.. sorry. Let me know if you find a solution.

using this code with other IMUs

Submitted by Fatma (not verified) on Sun, 2011-04-17 10:27.

we're using the processing ide code ,with the off chip filter to try and validate our data...
I'll paste the code here... in the processing ide we changed the usb port to 'COM11' and we even tried Serial.list()[0] since we only have one port
(the port is actually a virtual serial port).
we tried changing the baudrate from 9600 to 57600 as well but it didnt work for us..
could please take a look at the arduino code and see if maybe we're sending data the wrong way or something...
because though we can use the serial port monitor on arduino to view our data (and it comes out fine...) we cant seem to get any data on processing ide...

#define GYRO 0x68 // gyro I2C address
#define REG_GYRO_X 0x1D // IMU-3000 Register address for GYRO_XOUT_H
#define ACCEL 0x53 // Accel I2c Address
#define ADXL345_POWER_CTL 0x2D
char buffer[12]; // Array to store ADC values
int gyro_x;
int gyro_y;
int gyro_z;
int accel_x;
int accel_y;
int accel_z;
int i;
int c;
char str[512];
void setup(){
// Set Gyro settings
// Sample Rate 1kHz, Filter Bandwidth 42Hz, Gyro Range 2000 d/s
writeTo(GYRO, 0x16, 0x0F);
//set accel register data address
writeTo(GYRO, 0x18, 0x32);
// set accel i2c slave address
writeTo(GYRO, 0x14, ACCEL);
// Set passthrough mode to Accel so we can turn it on
writeTo(GYRO, 0x3D, 0x08);
// set accel power control to 'measure'
writeTo(ACCEL, ADXL345_POWER_CTL, 8);
//set accel right justified range to 16g
//cancel pass through to accel, gyro will now read accel for us
writeTo(GYRO, 0x3D, 0x28);
// Write a value to address register on device
void writeTo(int device, byte address, byte val) {
Wire.beginTransmission(device); // start transmission to device
Wire.send(address); // send register address
Wire.send(val); // send value to write
Wire.endTransmission(); // end transmission
void loop()
{// Read the Gyro X, Y and Z and Accel X, Y and Z all through the gyro
// First set the register start address for X on Gyro
Wire.send(REG_GYRO_X); //Register Address GYRO_XOUT_H
// New read the 12 data bytes
Wire.requestFrom(GYRO,12); // Read 12 bytes
i = 0;
{ buffer[i] = Wire.receive();
//Combine bytes into integers
// Gyro format is MSB first
gyro_x = buffer[0] << 8 | buffer[1];
gyro_y = buffer[2] << 8 | buffer[3];
gyro_z = buffer[4] << 8 | buffer[5];
// Accel is LSB first. Also because of orientation of chips
// accel y output is in same orientation as gyro x
// and accel x is gyro -y
accel_y = buffer[7] << 8 | buffer[6];
accel_x = buffer[9] << 8 | buffer[8];
accel_z = buffer[11] << 8 | buffer[10];
sprintf(str, "%d,%d,%d,%d,%d,%d,%d", accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z);
Serial.print(10, BYTE);
// delay(400); // wait for a second

p.s the basic code we got from hobbytronics

Hi there, thanks for stopping

Submitted by fabio on Sun, 2011-04-17 14:01.

Hi there, thanks for stopping by..

The Arduino code you added here uses 9600 speed.. so you have to use that too on the processing program.

I would suggest using the code in the attachment above called "Arduino Code IMU 6DOF ADXL345 ITG3200" which should work with the associated Processing code. Remember to change the address of the sensors in the I2C code to match the setup in your imu.

Btw, which IMU are you using?

thanks for your quick

Submitted by Fatma (not verified) on Mon, 2011-04-18 10:05.

thanks for your quick reply
we're using the IMU 3000 with adxl 345 ... we got it to work finally... simply had to remove the 256 and 14.35 from the processing side...
your codes been a great help for us in visually validating our imu data...
Thanks again.

code for imu3000 with adxl345

Submitted by dheeraj (not verified) on Thu, 2013-04-18 20:15.

i am also using the same fusion board so can u mail me the code plzz

Hey again Fabio! So, ive got

Submitted by Becky (not verified) on Thu, 2011-04-14 19:20.

Hey again Fabio!

So, ive got to code to work thanks to you, but now i need to correct it. The directions are completely wrong for me. Do you know what i need to change in my code please? For example, for the cube to be flat in the program i need to place my device upside down! Can you check my code please as see what's wrong?
Highly appreciate it!

Without knowing specifically

Submitted by fabio on Fri, 2011-04-15 08:38.

Without knowing specifically the devices you are using is hard to tell what's wrong in your code. The only thing I would suggest you to check is the fact the the gyro and accelerometer axis on your board aren't aligned, this means that you will have to align them in software.

Sparkfun guys are used in not aligning sensors axis on their boards: I always thought about this as non sense as it adds unneeded complexity without a clear advantage to the user.

Hey again fabio! Sorry i

Submitted by Becky (not verified) on Fri, 2011-04-15 17:34.

Hey again fabio!

Sorry i forgot to add the board im using before! Its this one:

And you're right! The axes arent aligned but through trial and error im getting the board to move in the right directions now :D
But i have a question, my yaw doesnt work at all! Was there something missing in the code i provided, or do i need to change something? (
Thanks for any help!

This code is for gravity

Submitted by fabio on Fri, 2011-04-15 18:21.

This code is for gravity sensing only.. this means that you won't have informations on the yaw angle, just picth and roll. There are however algorithms which uses accelerometer + gyroscope for yaw, pitch, roll sensing but they suffers from drifting on the yaw angle. See for example the algorithm described in this paper (you will also get some code there).

In order to fix the yaw drifting you need another reference vector, the Earth north sensed by a magnetometer. So, only your board won't be enough. You need to add a magnetometer breakout board or evaluate 9 degrees of measurement (DOM) boards like my FreeIMU.

If i would have known much

Submitted by Becky (not verified) on Fri, 2011-04-15 19:30.

If i would have known much earlier about this yaw drifting i would have gotten the 9DOF IMU instead as it seems much better but unfortunately i dont have enough time to change things around :(
Hrmm, regarding the algorithm, i went through the document but, is there an easier way to get a yaw output instead of through matrices and all the complex calculations? As it seems so difficult :/ Maybe you know another document regarding this subject please?
Im sorry if im such a pest, i just rly need some help :(

Everyone, including me will

Submitted by fabio on Fri, 2011-04-15 22:50.

Everyone, including me will need help from somebody else, sooner or later.. I think that if I help you now, "karma" will provide me with help when I'll need that. Moreover, I feel good helping people.. so you are not a pest at all.

Give a look at the code posted here. You'll find the FreeIMU library which will contain all the sensor fusion algorithm for doing a 9 DOM sensor fusion, which would need a magnatometer.

However, I think that you can just comment out the magnetometer part in the AHRSupdate function in FreeIMU.cpp to only use acc+gyro.

Hehe thanks fabio! Yea, i

Submitted by Becky (not verified) on Sat, 2011-04-16 17:16.

Hehe thanks fabio! Yea, i just end up feeling kinda bad when asking for help!
And thanks for that 9DOF website! Im gonna try implement it in my code and get back to you soon :) Thanks again for helping!

My gyro, is not giving me appropriate data

Submitted by ali (not verified) on Tue, 2011-04-12 19:34.

this is my code for Accelerometer and Gyroscope:
could you check the part about GYRO and see if there is something wrong or not?
when I am at 90 degree, it gives me very big values like 400 degree.
the clock for timer is 1MHz.

Please, when posting on

Submitted by fabio on Tue, 2011-04-12 21:21.

Please, when posting on pastebin, choose Never as expiration. Resubmitted to

A gyroscope measures angular velocity, not an absolute angle. I'm not sure what you mean.

I did integrate the data from

Submitted by ali (not verified) on Wed, 2011-04-13 09:30.

I did integrate the data from gyro. But the data I get it does not look right?
I turn on the timer and then I multiply the timer value to gyro reading and adding that to previous value we had for angle.

wireless IMU

Submitted by Fernando Jorge (not verified) on Sat, 2011-04-09 18:20.

Hi everybody!
Great job, Fabio. I'm having a lot of fun playing with your code and it works very fine for me. Many thanks for that :)
Now I'm trying to use a wireless connection through XBee radios, and checking the serial lines everything is ok, but starting up with the Processing code I get an "out of range" error message on line
"inString = inString.substring(2, 10);"

Do you have any idea what might be happening?

Many thanks in advance.

how to find out gyro conversion values

Submitted by Anonymous (not verified) on Fri, 2011-04-08 08:28.

we have a digital gyro and we need to figure out how to covert the data.
I've found the following equation:
Rt = SC × (Rm - R0)

Rt (dps): true angular rate
Rm (LSBs): gyroscope measurement
R0 (LSBs): zero-rate level
SC (dps/LSB): sensitivity

but how do we get zero rate level from the gryo datasheet??


You won't get that from the

Submitted by fabio on Fri, 2011-04-08 11:49.

You won't get that from the gyro datasheet as it is gyro specific. You can simply average a number of readings (eg: 64) while keeping the gyro steady and use that as R0.

thanks =D

Submitted by Anonymous (not verified) on Fri, 2011-04-08 14:35.

thanks =D

could you measure the period of your measuring loop?

Submitted by ali (not verified) on Thu, 2011-04-07 22:06.

I am reading my gyro and integrate my input and it takes me 0.036~0.039 second. I am using 1MHZ clock. Does this mean that my code is not efficient? could you measure the loop that you measure your gyro in all three axis and integrate? and could you state your clock if you did that?

I don't have specific timing

Submitted by fabio on Fri, 2011-04-08 14:59.

I don't have specific timing for only the gyro.. but I can say that reading an ADXL345, ITG3200 and HMC5843 and doing sensor fusion of them takes us 5 ms using a regular Arduino which runs at 16Mhz.

Thanks and another question

Submitted by ali (not verified) on Sun, 2011-04-10 21:22.

Thanks for your answer.
I am facing new problems, firstly I got your codes, and extract them but they are in tar format and I do not have any idea how to open them.
The second problem is with ADXL345, I read the datasheet but I am not sure about it. How should I make my ADXL345 to give me the data for one reading from all 3 axis at the same time. Now I just write 8 in 0x2D POWER_CTL register and it works but I am no so sure am I reading all data from all 6 axis for the same time or not. The other issue is that I am using 3.3 V and in datasheet they did not mention anything about mg/LSB at 3.3 Volt and they did not say anything about %/°C for 3.3 Volts. I think you used 3.3 volt so how did you solve the problem with mg/LSB and %/°C.
Any other thing that you think I have to take care of in order to get better result from ADXL345? I appreciate if you share your experience.
Thanks, like always you helped me out.

1 - Just get a serious

Submitted by fabio on Sun, 2011-04-10 22:56.

1 - Just get a serious archiving tool like 7-zip if you are under Windows and use that to open the code archives.

2 - You can't read all the three axis data from a single reading, but only with one big subsequent reading. The code posted in the article does exactly that.

Regarding the 3.3v issue, you may want to use a simple calibration procedure.. Place the accelerometer with one axis pointing up. Write down the reading. Place the accelerometer with the same axis pointing down. Write down the reading. Now you have the +1g and -1g interval values from the readings you wrote down. Divide the interval in two to compute the axis offset then use reading when pointing up or down + offset as LSB/g.

Hope this helps,


I used 7-Zip, win RAR, and

Submitted by ali (not verified) on Mon, 2011-04-11 07:44.

I used 7-Zip, win RAR, and Win Zip but after I extarxt the file it comes with .tar format I do not have any idea why.
Could you send them to my email or any other sulotion that you have in your mind. I know it sounds stupid but I really tried to open them and I could not.
Thanks a lot for all your helps. I will try LSB/g today.

Once you have the .tar file

Submitted by fabio on Mon, 2011-04-11 11:25.

Once you have the .tar file you can open that again with 7-zip .. .tar files are also archives.

Hi I am sorry, I do not try

Submitted by ali (not verified) on Mon, 2011-04-11 14:30.

I am sorry, I do not try to distract you, but seriously I can not open the file. I can open licence but the other file is in PDE which I cannot open it. Is that because I am using windows XP?
I am sorry, to waste your time for this kind of question... :(

That file is either an

Submitted by fabio on Mon, 2011-04-11 14:59.

That file is either an Arduino file or a Processing file which you should open with the respective IDE.

sorry to waste your time but

Submitted by ali (not verified) on Mon, 2011-04-11 18:21.

sorry to waste your time but I am not using Arduino, I am using Atmega 16. I thought I can change your code to C and use them for my micro. But anyway I thing that is not going to work.
I tried my accelerometer and I have new question, how should I find mg/C?
should I care about it? did you care about it in your codes?
thanks for fast replies.

Just use any text editor to

Submitted by fabio on Mon, 2011-04-11 18:56.

Just use any text editor to open the files, I would suggest ConTEXT if you are running Windows.

mg/C? What do you mean?

The ratio between temperature

Submitted by ali (not verified) on Mon, 2011-04-11 19:45.

The ratio between temperature and LSB. I mean that, does temperature affect my sensors output? If yes how can I deal with it?
I read in datasheet that the mg/C is equal to +-1 at 25C and 2.5 V, It did not make sense for me, that how this value is affecting the readings. I thought you read temperature from ITG3200 in order to get an exact value from sensor. Am I right?
I saw your codes; I can just say fantastic…. I think it will answer many of my questions without asking you.

I didn't do any temperature

Submitted by fabio on Mon, 2011-04-11 19:55.

I didn't do any temperature compensation in the codes posted in this article. There is work in progress on implementing that on the ITG3200 arduino library. See

Thanks a lot, for all your

Submitted by ali (not verified) on Mon, 2011-04-11 20:16.

Thanks a lot, for all your help. Can I share my code later when I am done with it and you improve it for me?


Submitted by fabio on Mon, 2011-04-11 21:56.

I can give it a review.

Thanksin this part I just

Submitted by ali (not verified) on Mon, 2011-04-11 22:02.

in this part I just read data from sensor.

admin edit: do not post code here directly. Use something like

sorry, next time I will post

Submitted by ali (not verified) on Mon, 2011-04-11 22:13.

sorry, next time I will post it there.
really tanks...

BTW it works. but is there

Submitted by ali (not verified) on Mon, 2011-04-11 22:10.

BTW it works. but is there anyway to make it faster?
and one question I did not see 2 complement in your code/or you had a way that I did not understand, could you explain about it. please.

I don't think you need to do

Submitted by fabio on Mon, 2011-04-11 22:19.

I don't think you need to do anything about handling 2's complement.. your microcontroller is probably already using that representation so once you assemble LSB and MSB you should be just fine. That's true for ATMEGA 328p on the Arduino.

at first glance it looks

Submitted by fabio on Mon, 2011-04-11 22:08.

at first glance it looks good.. do you have any specific question?

Hey Fabio, Quick question, i

Submitted by Becky (not verified) on Thu, 2011-04-07 14:17.

Hey Fabio,

Quick question, i connected my hardware like this ( (look at the third image from that page)). Would your code still work for my device when refering to the hardware or do i need to change something in the code to connect them properly?
Thanks a lot!

Hi Becky, thanks for stopping

Submitted by fabio on Thu, 2011-04-07 15:39.

Hi Becky, thanks for stopping by..

Unfortunately not.. you are using different sensors then mine so you have to adapt this code for them. Specifically, I'm using digital sensors while you have analog sensors. You may be interested in seeing this code:

I based my code on the code there and, as Starlino also was using analog sensors, I think you may use that as a reference.

Hey Fabio, This is probably

Submitted by Becky (not verified) on Wed, 2011-04-13 11:55.

Hey Fabio,

This is probably a really stupid question but please bare with me as i dont have much experience with coding! Can you confirm im writing the pins properly please? As i think they're wrong :S
In my code i wrote the following:

#define cGyroGXPin 0 //1x
#define cGyroGYPin 1 //1x
#define cGyroGZPin 2 //1x
#define cGyroGXPinAmp 3V3 //4x
#define cGyroGYPinAmp AREF //4x
#define cGyroGZPinAmp GND //4x
#define cGyroAXPin 3
#define cGyroAYPin 4
#define cGyroAZPin 5

And i connected my pins like this (third picture)

Am i doing something wrong?

I think they are correct. You

Submitted by fabio on Wed, 2011-04-13 15:06.

I think they are correct. You should share the whole code to help you more.

Post new comment

The content of this field is kept private and will not be shown publicly.
If you have a personal or company website insert its address in the form .
  • Web page addresses and e-mail addresses turn into links automatically.
  • Allowed HTML tags: <a> <em> <strong> <cite> <code> <ul> <ol> <li> <dl> <dt> <dd> <pre> <small> <del> <img> <h2> <h3> <h4> <b> <video> <sub> <sup>
  • Lines and paragraphs break automatically.
  • Images can be added to this post.
  • You may use [inline:xx] tags to display uploaded files or images inline.
  • You may insert videos with [video:URL]
  • Each email address will be obfuscated in a human readable fashion or (if JavaScript is enabled) replaced with a spamproof clickable link.

More information about formatting options