Introduction: 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.

Now your are ready for reading some data from 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:
gyroRate = (gyroAdc-gryoZero)/1.0323

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:
accVal = accAdc-accZero

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.

If you have any question, fell free to post a comment below.

Sources:
https://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.    


Newest firmware
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
Please consider backing the project.

Comments

author
AlexanderKielland (author)2016-10-27

Hi Lauszus,

Thank you for your code it has really helped me understand how the Kalman filter works.

I but i just cannot get my head over how you arrived at your estimated covariance matrix. I thought

P = F*p*F^2 + Q, where F =[1,dt; 0,1] and

Q =[dt^3/3,dt^2/2;dt^2/2,dt]*var^2

But this doesnt work in practice, like when i program it. How did you arrive at you covariance estimation in step 2? What formulas did you use?

thanks!

author
rajmohan747 (author)2016-10-17

In the article above, 3 axis accelerometer and gyro are used, right?

In that case what will be the equation for calculating YAW?just like the following equations for roll and pitch..

pitch = atan2(accYval, accZval)+PI
roll = atan2(accXval, accZval)+PI

author
Lauszus (author)rajmohan7472016-10-20

You can find more information on how to calculate yaw in my bachelor's thesis: https://github.com/Lauszus/LaunchPadFlightController/blob/master/docs. The relevant code is located here: https://github.com/Lauszus/LaunchPadFlightController/blob/master/src/IMU.c#L76-L93.

Regards
Kristian Sloth Lauszus

author
Ilovecars_mukut (author)2016-08-20

Can the code be used for a self balancing robot? There is no setup of motors in the code...i think its not the code for balancing...it would be great if u could upload it to github and post the link or just send me at - ilovecars252003mukut@gmail.com

Actually I am just 13 years old and I have made several robots but easy ones.. I want to implement ROS so I wanted to understand KF and EKF so thought of doing this project. Hope you can help me!

author
Lauszus (author)Ilovecars_mukut2016-08-22

Please see my blog post: http://blog.tkjelectronics.dk/2012/03/the-balancing-robot.

author
MuhammadR161 (author)2016-06-25

how to calibrating mpu 6050 with arduino use trigonometri formula?

author
NaqashK (author)2016-06-13

To Find more you can found formulas description here

https://www.instructables.com/id/Accelerometer-Gyro...

author
NaqashK (author)NaqashK2016-06-13

Found has to be corrected by Find :p

author
AmrizalA (author)2016-04-05

Hello Lauszus,

how to change the max angle of pitch in your kalman code for mpu6050 from 90 degree to 180 degree?

thankyou

author
Lauszus (author)AmrizalA2016-04-08

Just comment out this line: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU/MPU6050/MPU6050.ino#L21, please also read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf.

Regards
Kristian Sloth Lauszus

author
AmrizalA (author)Lauszus2016-04-08

Why in your code kalman mpu6050 with hmc585L if i serial monitor showing i2c write failed 2
But the connection with scl and sda is true why?
Arduino mega

author
Lauszus (author)AmrizalA2016-04-14

Make sure that you have connected the sensor correctly. Have you remembered to connect VCC and GND as well?

author
GeoffreyC7 (author)2015-11-21

Hello Lauszus,

This is an excellent article with extraordinarily precise code. That said, I am working on a project that requires data from to gyros to the Arduino simultaneously. Where in the code can change the analog inputs? If there is a better way to do it, i'm all ears.

Thanks

author
Lauszus (author)GeoffreyC72015-11-22

You can change them in the top of the code here: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/d140efc95d7cc98643f5cdfe9cd543d7328bf28d/IMU/LPR530AL_LY530ALH_ADXL335/LPR530AL_LY530ALH_ADXL335.ino#L23-L29.

author
GeoffreyC7 (author)Lauszus2015-12-02

Lauszus,

I appreciate the timely response and I have been looking through the code available for download in this article as well as the one you sent me a few days ago. The code posted on this article, has worked on my arduino and has been compatible with the IMU 9150 I have been using. Here is the hookup I have been using:

IMU--> Arduino:

GND--> GND

VCC--> 3.3V

SDA--> A4

SCL--> A5

So far this setup has been working for the code downloaded from this article and I am able to cleanly receive dependable data from the gyro, accelerometer, and the combined Kalman calculated filter. That said, based off the code you gave me a few days ago, I do not understand how to properly hookup this IMU 9150 for those parameters, or how to alter the code you sent to satisfy the IMU 9150 setup. I was thinking that perhaps there is a way to change the analog input on the code posted in the article, or there is a way to modify the code you sent so it is compatible with the technology I have. If necessary I may be able to purchase the IMU Analog Combo Board Razor, though I am unable to find it available online, but I would like to try to resolve this problem with the equipment I have at this time.

I appreciate the time and effort you are putting in to help me resolve this problem.

Thanks, Geoffrey

author
Lauszus (author)GeoffreyC72015-12-06

You should take a look at the following example I have provided for the MPU-6050: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/MPU6050. If you want to use the magnetometer inside the MPU-9150 you should have a look at the following drivers for my flight controller: https://github.com/Lauszus/LaunchPadFlightController/blob/master/src/MPU6500.c and https://github.com/Lauszus/LaunchPadFlightController/blob/master/src/AK8963.c, as the MPU-9150 has a AK8975 connected to it's external I2C pins, which is properly very similar to the MPU-9250 (MPU-6500 with AK8963) which is used on the flight controller.

author
GeoffreyC7 (author)Lauszus2015-12-08

Lauszus,

The program for the MPU 6050 is the one I have been using, with the setup I previously mentioned. Is there anyway I can use multiple IMUs (gyro and accelerometer with the kalman filter) using this code? This is why I was wondering how to change the analog input. I want to be able to run multiple IMUs with one program and arduino.

Thanks for all the help,

Geoffrey

author
Lauszus (author)GeoffreyC72015-12-15

You can hook up two sensors by connecting AD0 to 3.3V and GND respectively. If you need more sensors than that you will have to use a I2C multiplexer like this one: https://www.adafruit.com/products/2717.

Sorry but no analog inputs are used when you use the MPU-6050. I guess you are referring to A4 and A5, right? Remember that almost any analog pin can be used as digital pins as well. The A4 and A5 on the Uno can be used for I2C as well. You should hook up both sensors to these two pins. For more information regarding I2C please see: https://learn.sparkfun.com/tutorials/i2c.

Regards

Kristian Sloth Lauszus

author
missmus (author)2015-09-26

Hi,

iI'm newbie..my project is about stabilizing...i want to stabilize the motor on moving platform by using 5dofIMU(SN-IMU5DOF-LC) and G15 Cube servo..using gyro..or should i use both gyro and accelerometer..but i'm really new and don't know how to do it..now i just got the reading of gyro and convert it to degree, then i want to combine the IMU and cube servo but don't have any idea how to do it.. can u help me..maybe with source code or how to do it...please help me.. :(

author
Lauszus (author)missmus2015-09-30

Sorry, but did you read the guide? It explains how to calculate the angle.

In order to control a servo you can simply use the Arduino servo library: https://www.arduino.cc/en/Reference/Servo.

Regards
Kristian Sloth Lauszus

author
OsamaA2 (author)2015-08-08

The best explanation that I have seen online so far.

Thx for the amazing effort.

author
vjyothi (author)2015-02-19

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

author
elarbi.ennair (author)2015-01-22

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

thanks in advance :)

author
jesterod. (author)2015-01-16

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

author
sufiyan.muhammad.9 (author)2014-12-20

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);

author
chenjian (author)2014-03-03

hello.bro. i can't find the 6DOF (six degrees of freedom) IMU board, i want to know ,if any another board could replace it, please

author
Lauszus (author)chenjian2014-03-14

You should take a look at the MPU-6050. I have written some code for it as well: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino.

author
erik.schnitzler (author)Lauszus2014-11-24

Hello Lauszus.
Can you re:post the guide for MPU-6050? It doesn't seem to work when you enter the link. Thanks in advance.
Erik

author
Lauszus (author)erik.schnitzler2014-11-24

I haven't written a guide, but I guess you are talking about the example code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/MPU6050?

author
CharithaS (author)2014-11-16

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???

author
tam nguyen (author)2014-08-26

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

author
chenjian (author)2014-03-03

wow.wow .that's really really cool

author
jeneral (author)2013-11-26

i try to disconnect all acc and gyro..
but the serial monitor still display 180.
the IMU has no effect on the arduino.


author
Lauszus (author)jeneral2013-11-26

Okay. Try another Arduino and if that does not work, then contact the place you bought the IMU.

author
jeneral (author)Lauszus2013-12-04

i try to insert the other coding, it seems that the IMU and the Arduino is ok..
maybe there is something i need to setup with the coding you provided.

another request..

can you help me to build arduino coding with condition like below..

the count starts from 0-90
i want to make my LED light on from count 5 until 20.. other value, the LED is off
the LED is using PWM with range 66-184(1.29v-3.6v)..

it means that..

count-->  5-20
PWM--> 66-184 (1.29v-3.6v)
so, 8.37pwm per count..

please help me..

author
Lauszus (author)jeneral2013-12-25

No I will not write the code for you, sorry.

author
jeneral (author)2013-11-26

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..

author
Lauszus (author)jeneral2013-11-26

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.

author
jeneral (author)Lauszus2013-11-26

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?

author
engum (author)jeneral2013-11-29

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?

author
Lauszus (author)engum2013-12-02

You should look up extended Kalman filter and DCM algorithm. Just try to Google "Camera stabilisation Arduino" and there should be plenty of resources.

author
jeneral (author)Lauszus2013-11-26

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..

author
engum (author)Lauszus2013-11-29

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?

author
jeneral (author)2013-11-22

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?

author
Lauszus (author)jeneral2013-11-25

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.

author
jeneral (author)2013-11-21

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


author
Lauszus (author)jeneral2013-11-21

Please send me a link to your IMU.
If looks like it is just a regular analog IMU, so simply connect it as described in this guide.

Regards
Lauszus

author
jeneral (author)Lauszus2013-11-22

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

About This Instructable

261,084views

202favorites

License:

More by Lauszus:Guide to gyro and accelerometer with Arduino including Kalman filtering
Add instructable to: