# Guide to gyro and accelerometer with Arduino including Kalman filtering

This guide was first posted at the Arduino forum, and can be found here: http://arduino.cc/forum/index.php/topic,58048.0.html

I just reposted it, so even more will get use of it. The code can be found here:
https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter

Hallo everybody
I recently bought this analog 6DOF (six degrees of freedom) IMU board (http://www.sparkfun.com/products/10010) from watterott.com. It uses three gyros and three accelerometers to calculate angles in three dimensions.

I looked a while for some code online and how to connect with them. After many hours of research I succeeded of making af precise measurement of angles in two directions. I decided to write a short guide for fellow electronic enthusiasts.
The main purpose of this guide is to teach others how to get some useful data from their IMU or just a gyro or accelerometer. The code for Arduino can be found at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter. It should be pretty easy to implement my code to your own sensor. I will not describe all the details about the theory behind, instead you can look at the sources for more info.

Before you begin you have to connect the IMU as follows:

Acc_Gyro          Arduino
3.3V        <—>   3.3V
GND        <—>   GND
Gx4 X      <—>   AN0
Gx4 Y      <—>   AN1
Gx4 Z      <—>   AN2
Acc X      <—>    AN3
Acc Y      <—>    AN4
Acc Z      <—>    AN5

Also connect 3.3V to the AREF pin on the Arduino for more accuracy.
It is VERY important that you do not connect the sensor to 5V - this will destroy the sensor.

To communicate with the sensor is straightforward:
The gyro measures degrees per second while the accelerometer measures acceleration (g's) in three dimensions. Both outputs the measurements as a analog signal.
To get these translated into degrees you have to do some coding:

The gyro
First you have to translate quids (a number from 0-1023) into something useful (this is for a ADC with a 10 bit resolution, for example this should be 4095 (2^12-1=4095) for 12 bit ADC). To do this I just use this simple equation:
gyroRate = (gyroAdc-gyroZero)/sensitivity - where gyroAdc are the readed value from our sensor, gyroZero is the value when it is stationary (this is done in the code - look in the "Setup" section) while sensitivity is the sensitivity found in the datasheet, but translated into quids.

If you look in the two gyros datasheets (http://www.sparkfun.com/datasheets/Sensors/IMU/lpr530al.pdf and http://www.sparkfun.com/datasheets/Sensors/IMU/LY530ALH.pdf) you will see that the sensitivity is 3.33mV/deg/s for the 4xOUT. To translate these into quids is pretty easy: sensitivity/3.3*1023.
So in this example I get:
0.00333/3.3*1023=1.0323.

NB: to translate mV to V simple just divide by one thousand.

The final equation will look like this:

The result will come out as degrees per second. To translate this into degrees you have to know the exact time since the last loop. Fortunately, the Arduino got a simple command to do so: millis(). By using that, one can calculate the time difference (delta time) and thereby calculate the angle of the gyro. The final equation will look like this:
gyroAngle += gyroRate*dtime/1000

Unfortunately, the gyro drifts over time. That means it can not be trusted for a longer timespan, but it is very precise for a short time. This is when the accelerometer comes in handy. It does not have any drift, but it is too unstable for shorter timespan. I will describe how to combine these measurements in a while, but first I will describe how to translate the readings from the accelerometer into something useful.

The accelerometer
The accelerometer measures the acceleration (g's) in three dimensions. To translate the analog readings into degrees you simply need to read the axis and to subtract the zero offset like so:

Where accAdc is the analog reading and accZero is the value when it reads 0g - this is calculated in the start of the code, look in the "Setup" section. The zero value can also be found in the datasheet: http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf. You will see that the zero voltage at 0g is approximately 1.5V, to translate this into quids, you again have to use this equation: zeroVoltage/3.3*1023.
So in this example I get:
1.5/3.3*1023=465.

You can then calculate the pitch and roll using the following equations:
pitch = atan2(accYval, accZval)+PI
roll = atan2(accXval, accZval)+PI

Atan2 has a output range from -π to π (see http://en.wikipedia.org/wiki/Atan2), I simply add π, so the range it converted to 0 to 2π.
To convert it from radians to degrees we simply multiply the result by 57.295779513082320876798154814105 - this is predefined in the Arduino IDE as RAD_TO_DEG.

Kalman filter
As I explained earlier the gyro is very precise, but tend to drift. The accelerometer is a bit unstable, but does not drift. You can calculate the precise angle by using something called a Kalman filter. A detailed guide on how it's implemented can be found at my blog: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/.

If you want to use something a bit more simple, you can use what's called a Complementary Filter. It is pretty easy to understand and the math is much simpler, because it only works in one step.
For example the equation could look like this:
angle = 0.98 *(angle+gyro*dt) + 0.02*acc - you can fine tune the numbers to whatever you like. Just remember that the sum must be 1.
For me the result from the Complementary Filter was very close (or almost the same) as the one calculated by the Kalman filter.

You have now learned (hopefully) how to get analog data from IMU and translate it to something useful. I have attached my own code for my 6DOF IMU (http://www.sparkfun.com/products/10010), but with some slightly modification, I am pretty sure that it is possible to use it with any analog gyro/accelerometer.

Sources:
http://www.instructables.com/id/Accelerometer-Gyro-Tutorial/
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418
http://www.x-firm.com/?page_id=148
http://web.mit.edu/first/segway/

Update
I have just finished a Processing code which prints out data from the Arduino on a nice graph. As you can see in the video below the filtering is quit effective. The light blue line is the accelerometer, the purple line is the gyro, the black line is the angle calculated by the Complementary Filter, and the red line is the angle calculated by the Kalman filter. As you might see the Kalman filter is just a bit more precise (i know it is difficult to see in the video) than the Complementary Filter, especially when I shake it.
I have attached my code, both the updated code for the Arduino and the Processing code. It is also possible to see the data from the y-axis. Just uncomment drawAxisY(); in the code.

I decided to put all the source code on github, as it is much easier to maintain.
The newest code can now be found at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter

My Balancing robot
Below is a video of my balancing robot. It uses the same IMU and algorithm as described in the post above.

Kickstarter
I have just released my balancing robot on Kickstarter: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit

 1-40 of 109 Next »
OsamaA223 days ago

The best explanation that I have seen online so far.

Thx for the amazing effort.

vjyothi6 months ago

Hi I wanted to use the IMU data with camera for vision based navigation,I need help regarding this,and IMU is 9DOF sensor stick from sparkfun and camera is Link sprite JPEG camera

elarbi.ennair7 months ago

Modeling accelerometer and gyroscope in simulink

Hi everyone , i'm working on a tracking system project that will localise people inside

a building during their mouvements using the IMU : inertial measurement unit (gyroscope

+ accelerometer) , and i have chosen the kalman filter algorithm to read the output of

the IMU to estimate and update the actual position

i need if it possible a module in simulink that simulate the gyroscope and accelerometer

and also how to implement the algorthm using kalman filter

jesterod.7 months ago

what part is the gyro i have an wireless remote/keyboard i want to disable the air-mouse gyro in well add a switch really

Hey, I am working on a project called wireless inertial pointer. Its a device just like a wireless mouse which can be used to control the cursor(pointer) on screen. I have used Arduino UNO, MPU 9150 and I am combining my gyro and accelerometer data using complimentary filter.

Everything is working well but my cursor on the screen is not moving smoothly, can you please help me with the mouse code.

my code for implementing mouse function is:

if(compAngleX > 90)

compAngleX = 90;

if(compAngleX <-90)

compAngleX = -90;

if(compAngleY > 95)

compAngleY = 95;

if(compAngleY <-95)

compAngleY = -95;

int x = map(compAngleY, -90, 90, -10, 10);

int y = map(compAngleX, -90, 90, -10, 10);

Mouse.move(x,y,0);

CharithaS9 months ago

Hi,

I am a newbie in this area. How is the accelerometer used for measuring angles when the subject is in dynamic condition. I'm confused about this fact???

Lauszus (author)  CharithaS9 months ago
tam nguyen1 year ago

Very detailed article and very useful, your post makes me more motivated to do the balancing robot. Thank you very much!

chenjian1 year ago

wow.wow .that's really really cool

jeneral1 year ago
i used the code given in "IMU6DOFVer2.zip".. i set the baud rate same with the coding, but the serial monitor display all values zero..

if i put the code as you give in link below,
https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/00c0bbfddbc27a302f29f22df8318959aa344435/IMU6DOF/MPU6050/MPU6050.ino#L39
then the error "sketch_nov24a:45: error: 'i2cWrite' was not declared in this scope"

now i confused which code i have to use..
i am totally zero..
Lauszus (author)  jeneral1 year ago
Sorry I got confused and send you the wrong link - that is why you got confused :)

The correct is this one: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/00c0bbfddbc27a302f29f22df8318959aa344435/IMU6DOF/LPR530AL_LY530ALH_ADXL335.
1 year ago
i used the code in link above.. the serial monitor now display all value 180 and not change..

is the code suitable with my IMU?
http://www.cytron.com.my/viewProduct.php?pcode=SN-IMU5D-LC&name=5%20DOF%20IMU

maybe i need to set something there?

1 year ago
Hello Sir! I want to make "9DOF IMU" application:
Camera control/stabilisation.
By using tri-axis gyroscope,tri-axis accelerometer,tri-axis megnetometer, and interfacing with Real-time control interface x-io board..kindly help please how i can buil this?
Lauszus (author)  engum1 year ago
You should look up extended Kalman filter and DCM algorithm. Just try to Google "Camera stabilisation Arduino" and there should be plenty of resources.
Lauszus (author)  jeneral1 year ago
1 year ago
Ya.. i have connected properly
Acc_Gyro Arduino
5V <—> 5V
GND <—> GND
Gx4 X <—> AN0
Gx4 Y <—> AN1
Acc X <—> AN3
Acc Y <—> AN4
Acc Z <—> AN5

when i try to print "accXval" and "accYval".. they display zero.
maybe that is the problem..
1 year ago
Hello Sir! I want to make "9DOF IMU" application:
Camera control/stabilisation.
By using tri-axis gyroscope,tri-axis accelerometer,tri-axis megnetometer, and interfacing with Real-time control interface x-io board..kindly help please how i can buil this?
jeneral1 year ago
ok, i connect
Acc_Gyro Arduino
5V <—> 5V
GND <—> GND
Gx4 X <—> AN0
Gx4 Y <—> AN1
Acc X <—> AN3
Acc Y <—> AN4
i dont use z-axis..

i just straight put in your code.. but the serial monitor display zero..
what the problem? do i need to set anything else before upload to arduino?
Lauszus (author)  jeneral1 year ago
Have you remembered to set the right baud rate in the serial monitor - take a look at the dropdown menu in the bottom right of the serial monitor.

It must match this value: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/00c0bbfddbc27a302f29f22df8318959aa344435/IMU6DOF/MPU6050/MPU6050.ino#L39.
jeneral1 year ago
hello.. help me.
how to connect my imu..
i have these pins:
1) 5v
2) GND
3) X-acc
4) Y-acc
5) Z-acc
6) X-ratex4.7(inv)
7) Y-ratex4.7(inv)
8) X-rate
9) Y-rate
10) ST-acc

Lauszus (author)  jeneral1 year ago
If looks like it is just a regular analog IMU, so simply connect it as described in this guide.

Regards
Lauszus
1 year ago
www.cytron.com.my/viewProduct.php?pcode=SN-IMU5D-LC&name=5%20DOF%20IMU

Lauszus (author)  jeneral1 year ago
Simply just connect it as explained in this guide. Except you should connect 5V to the IMU instead of 3.3V. Also you can not measure yaw using it, as only have a 2-axis gyroscope.
kgupta111 year ago
hello
first of all thanks for such clear and simple instructions.
you wrote that the gyro readings are precise but it drifts away with time, my question is in my project the gyro's orientation will be changing in almost every second to every minute(max), is the drifting problem still an issue for my application i.e. does the value drifts away when the orientation is kept same for long or does it drifts anyway on very long periods of use, and also what is the approximate period for significant drift.

Lauszus (author)  kgupta111 year ago
Hi, The amount of drift you will get depends on your gyroscope, so will have to do some measurement yourself and test it.

If the filter is properly tuned then the output from the Kalman filter is very precise.
You should have a look at the following blog I wrote: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/.

Regards
Lauszus
1 year ago
well i have MPU-6050, and thanks for the blog link!
Lauszus (author)  kgupta111 year ago
I have written an example specific for that sensor. You can find it here: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino

Regards
Lauszus
mit2sumit2 years ago
ok, after calculations, and getting the values, how is it supposed to control the speed of my quad and make it hover...I just can't able to visualize in implementing the gyros and accs...
Sorry for my dummines :confused:

need fast and better sugestions...
Lauszus (author)  mit2sumit2 years ago
Try to Google PID controllers. That's how I would do it. You can find plenty of existing firmware already online.
Saugpt19912 years ago
i don't y,i'm confused for PID.
Are these the steps of PID Tuning?
dv=kp*angle+ki*integral+kd*diff;
integral+=angle;
diff=angle-prev_angle;
prev_angle=angle;
oc0=(int)dv;
Lauszus (author)  Saugpt19912 years ago
Take look at these lines: https://github.com/TKJElectronics/Balanduino/blob/8a7955b9a96e24fd50034f70927c5b58e8966d5e/Firmware/Balanduino/PID.ino#L45-L53 I use for my balancing robot.
You will need to find the Kp, Ki and Kd values.
Saugpt19912 years ago
Hello sir,
once again! i'm here with you.
As i told you in my previous conversation, i was trying to make the self balancing robot using AVR platform but some of the problem.My robot was not working properly so again i need your help.
I got the PID result on my MATLAB Software but unfortunately,i'm not able to tune the graph of PID so I need your help to tune the parameter of Kp,Kd,Ki of PID to balance the robot.
if u know any software to direct tune PID using graph then it will be really helpful for me.
Lauszus (author)  Saugpt19912 years ago
Demido2 years ago
Hello Sir. First of all, congratulations for the job done (and time spent) to explain step by step your self balancing project. I live in France, so, please, be patient with my Englsh. I don't know if I write this message in the right place, but I'll write it anyway; maybe you'll see it. I intend to built a self balancing scooter. I want to mix the Segway with the Toyota Winglet. I mean 2 big motor-wheels (12" diameter, 1500W each, 48 to 60V batteries with 20 to30Ah) mounted on two leaning half platforms. It will be made for private use only(my wife and me). I allready designed the mechanical part of the thing (no name yet), but the electronics is very new for me, and there I have questions to ask. Two months ago I started to learn programation with Arduino. So, if you think that my project is reliable and you have, from time to time, a few minutes for me, answer this mail, please. Best regards R. DOMIDE
Lauszus (author)  Demido2 years ago
I don't have any experience with balancing scooters, but the principle is the same. Perhaps you should try to build a smaller version like a balancing robot first and then build the large one?
Also check out my Kickstarter campaign: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit. It's a complete kit with all you need to build a balancing robot.
vkarpuram2 years ago
How would I do this with the arduIMU V3? The pinouts are completely different from the razor IMU.
Lauszus (author)  vkarpuram2 years ago
It uses a digital sensor instead. The accelerometer and gyroscope is the MPU-6000. Check out this code I have written for the MPU-6050: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino.

The only difference between the MPU-6000 and the MPU-6050 is that the MPU-6000 supports SPI as well. So you can use my code just fine.
eggshot2 years ago
Thanks for your advice but I was able to do it without wheel encoders, putting the IMU at the top portion, the battery at the bottom and with just a PD loop(I could not figure out how to implement the integral part, as it will keep on adding the errors resulting to a maximum dutycycle). It's stable on rough surfaces but not with smooth floors. Although it's about 15 degrees of tilt limit(maybe due to the insufficient speed of the motors and the improper positioning of the components especially on concentrating the weight on the bottom), atleast it can do balancing by itself.