## Guide: Gyro and Accelerometer Kalman filtering, with the Arduino

Hello again everybody.

Today I agreed with my fellow classmate and team member, Kristian Lauszus, to post his guide to Kalman filtering, using the Arduino with a Gyro and Accelerometer, on my blog. So here it is, and I think it will be usefull for your guys.

Questions can be asked in the Arduino forum where the Guide was originally posted: http://arduino.cc/forum/index.php?topic=58048.0

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(accXval^2+accYval^2+accZval^2).

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 or in the Arduino forum: http://arduino.cc/forum/index.php?topic=58048.0

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.

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

very good guide how to use gyro and accelerometer sensors. I add this tutorial on my article where a long list with sensors and tutorials about how to interface and programming accelerometer, gyro and IMU sensors

@Ezu

Thanks! You might want to bookmark the forum post as well: http://forum.arduino.cc/index.php/topic,58048.0.html.

I originally posted on the forum, but then decided to post it this blog as well 😉

brilliant!_!_!

I’d like to use Graph the way you did, for measuring and graphically monitoring the outputs with my

Mac, but I’ve never found the way. Could you give me some reference IOT understand how to do it? Thanks in advance

Ale

@alessandro

You can find the newest version of the code here: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050/Graph. Simply download the repo and then find the executable application that matched your operating system.

If you want to change the code you will have to install a program called Processing: http://processing.org/.

Lauszus,

many thanks! I’m going to download right now.