3 Simple Ways to
Share What You Make

With Instructables you can share what you make with the world — and tap into an ever-growing community of creative experts.

PhotosPhotos

Share one or more photos of a project, recipe, or whatever you've made, quickly and easily.

Step by StepStep-By-Step

Share your step-by-step photos with text instructions of what you made so others can do it too!

VideoVideo

Share your how-to video. You'll need your embed code from a video site such as YouTube.

Guide to gyro and accelerometer with Arduino including Kalman filtering

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://rapidshare.com/files/1073395588/IMU6DOFVer2.zip
https://rapidshare.com/files/3588312346/IMU6DOF.zip
https://rapidshare.com/files/4139847836/Graph.zip

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. I have attached my code for Arduino. 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 (0/s) while the accelerometer measures Earth gravitational acceleration (g) 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 10 bit resolution, for example this should be 4095 for 12 a ADC bit module). 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 horizontal (this is done in the code - look under "Calibration") 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/0/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 (0/s). 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 and thereby calculate the angle of the gyro. The final equation will look like this:
gyroAngle=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 gravitational acceleration (g) in three dimensions. To translate the analog readings I use this equation:
accVal = (accAdc-accZero)/sensitivity - where accVal are the g's measured by the sensor, accAdc is the analog reading, accZero is the value when it is horizontal (this is done in the code - look under "Calibration"). Sensitivity is the sensitivity found in the datasheet, but translated into quids.
This is done the same way as with the gyro. If you look in the datasheet (http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf ) you will see that the sensitivity is about 270-330 mV/g. I use the top one (330mv/g) because the the Vs is connected to 3.3V and not 3V. You might want to find out what your true sensitivity is by experimenting.

To translate this into quids again use this equation: sensitivity/3.3*1023.
So in this example I get:
0.33/3.3*1023=102,3.

The final equation will look like this:
accVal = (accAdc-accZero)/102,3

To calculate the angle you first have to calculate the force vector (see http://www.instructables.com/id/Accelerometer-Gyro-Tutorial/#step1  for information) to do so I just use Pythagoras in three dimensions:
R = sqrt(accXval2+accYval2+accZval2).

You can then calculate the angle between the x-axis and the force vector (R) by using acos (inverse of cosinus):
accXangle = acos(accXval/R)
It is the same with the y-axis:
accYangle = acos(accYval/R)

The output will be as radians. To tranform it into degrees you just multiply the result with 57.295779513082320876798154814105.

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. I do not know the exact theory of how it works, but a simple explanation is that it combines the two calculated angles and then "guesses" the true angle with mathematics.

Basically it works in three steps:
First it gets the accelerometer angle
It then compares it with the angle calculated by the gyroscope as well as the last angle
Outputs the estimated angle based on the previous step

I did not make my own Kalman filter, so I can not tell exactly how it works, instead I used the Kalman filter from this project: http://www.x-firm.com/?page_id=145 .

If you do not like using something you do not understand, you can use something 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 attach 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.

What do you think? Did I get something wrong or any ideas for improvements?

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.    


My Balancing robot
Below is a video of my balancing robot. It uses the same IMU and algorithm as described in the post above.
24 comments
Feb 25, 2012. 12:22 PMgaurav_sharma132 says:
first if all, It was great help Lauszus. Thanks a ton.
Secondly, I want to develop quadrotor. I'm newbie but I started learning it.
1. How many IMU DOF is minimum required for it ?
2. Can I use Arduino Uno for it ?
will this solve my purpose?

Feb 14, 2012. 10:56 PMmonjidj says:
can i make segway useing this analog 6 dof sensor ...after make this connection should i need to connect potentimeter to arduino 5v on pot to 5v on arduino
0v on pot to 0v on arduino and middle pot to any analog of arduino ....and then conncet the s1 and 0v on sabertooth to pin 5 on pwm arduino and pin 0 arduino ...is that enough ....? is that all? can any answer
Feb 17, 2012. 8:16 PMmonjidj says:


while trying to search to built my owen segway ....i bought to accomplished my project .

1-Sabertooth 2x25 motor controller http://www.dimensionengineering.com/datasheets/Sabertooth2x25.pdf
2-imu razor combo analog 6 dof sensor http://www.sparkfun.com/products/10010
3-arduino mega 2560..(micro controller )
http://arduino.cc/en/Main/ArduinoBoardMega2560


kindly, could you have look to this connection on link below ..between the arduino and sensor?

http://voidbot.net/razor-6dof.html


could you help me find the best connection between the sabertooth and the micorcontroller ?

and then i'm thinking (( Just ))connect S1 and 0v on sabertooth to to pin 4 on arduino (for example) and ground on arduion again...
and ignore S2, and 5v.....? what do you think ?

do you can make me small diagram??


Feb 18, 2012. 9:25 PMmonjidj says:
Mr Lauszus,

you know ...so far and as far as i know .. you the first useing the analog 6 dof sensor to built segway...it will be great experience..
.please put me in touch if you got website ...or youtube...show the steps...all the best.

becouse i use the arduino mega 2560..the connection to sabertooth
Sabertooth - Arduino
0V - GND
S1 - PWM pin2
S2 - PWM pin3
Is that right?

also to filter the signal from pwm useing 10k ohm ressistance ..should i take two ground from the arduino....and connect the ressistace in between...or one ground and connect all togather ....? or my quistione confused you...

Thanks...


..
Feb 13, 2012. 3:12 PMflorinm82 says:
Hello Lauszus,

Would this 6 DOF Kalman filtering work fast enough in order to provide roll stabilization for a sounding rocket? Would this code also work with other accelerometer/gyros (especially ones resistant at high g's)? What should be changed in the code if one changes the type of accelerometer/gyros?

In a sounding rocket we are talking of speeds ~ 600 m/s (Mach 2) and accelerations (during boost phase) of about 30g's.

Of course the roll stabilization would only be needed during the coast phase, when the motor already burnout, so the acceleration is way smaller (probably around 2-3 g since it is generated only by gravity and an ever decreasing drag force).

Best regards.
Feb 13, 2012. 3:19 PMflorinm82 says:
How would you connect a GPS with the IMU in order to make a full autopilot?
As it stands right now the IMU just gives you attitude information.

How would you blend the GPS data into the Kalman filtering in order to give you full autopilot capabilities?

Best regards!
Feb 7, 2012. 10:10 PMSureshJonna says:
I thank you for this wonderful post.
Jul 5, 2011. 6:29 AMprateekk says:
hi . ur content is gud really helpful but can u plz tell me that once i get all thinghs done how should load a code into my board..? i mean is there any software which might help me to feed a programe into my circuit.!!
Aug 23, 2011. 6:24 PMT-D-M says:
Hi Lauszus,PS nice job
I have got a bit of a hick-up i wanted to try your code with a 5DOF from http://www.sparkfun.com/commerce/product_info.php?products_id=741
getting rid of the zgyro, but im having troubles because the gyro and accelerometer are not configured as yours, I tried inverting the gyro but to no avail,
I cannot get any results from the graph, please help
Aug 23, 2011. 10:20 PMT-D-M says:
Dont bother all sorted,thanks once again for this Osum post,well done.
Hows your balancing robot going anyway?
Aug 29, 2011. 7:07 PMT-D-M says:
Hey Lauszus
Do you mind telling me why we are dividing dtime by 1000,thanks in advance
Feb 7, 2012. 10:10 PMSureshJonna says:
This may be a stupid question:

What is dtime?

Pro

Get More Out of Instructables

Already have an Account?

close

All Steps Viewing
View all steps of an Instructable on the same page when you're a Pro Member.

Upgrade to Pro today!
2
Followers
1
Author:Lauszus