Starlino
Home Subscribe Contact

Arduino code for simplified Kalman filter. Using a 5DOF IMU (accelerometer and gyroscope combo)

January 22, 2010

This article introduces an  implementation of a simplified filtering algorithm that was inspired by Kalman filter. The Arduino code is tested using a 5DOF IMU unit from GadgetGangster - Acc_Gyro . The theory behind this algorithm was first introduced in my Imu Guide article.

The Acc_Gyro is mounted on a regular proto-shield on top of an Arduino Duemilanove board.

IMG_1359.JPG

 

Parts needed to complete the project:

- Arduino Duemilanove (or similar Arduino platform)
- Acc_Gyro 5DOF IMU board
- Protoshield (optional)
- Breadboard
- Hook-up wire 22AWG

The hook-up diagram is as follows:

Acc_Gyro <--->  Arduino
5V          <--->  5V 
GND      <--->  GND
AX       <--->  AN0
AY       <--->  AN1
AZ       <--->  AN2
GX4      <--->  AN3 
GY4      <--->  AN4  

Once you have completed the hardware part, load the following sketch to your Arduino.

imu_arduino.zip

Run the project and make sure you are receiving an output on your serial terminal (you can start the terminal from your Arduino IDE).

To analyze the data I have developed a small utility called SerialChart. It is open-source so feel free to customize it for your own needs.

Here is the output from SerialChart software:

imu_arduino_serial_chart.png

 

The test was performed as follows:

- first I was tilting the board slowly (marked "smooth tilting" on the screenshot)

- next I continued tilting the board, but I also started applying some vibration - by tapping the board quickly with my finger (marked "Titlting with vibration noise")

As you can see from the chart the filtered  signal (red line)  is indeed more immune to noise than the accelerometer readings alone (blue line). The filtered signal was obtained by combining the Accelerometer and Gyroscope data. Gyroscope data is important, because if you would simply average the Accelerometer data you would get a delayed signal. Given the simplicity of the code and of the algorithm I am satisfied with the results. One feature that I would like to add is compensation for the drift effect that you might encounter with some gyroscopes. However the Acc_Gyro board proved to be very stable in this respect, since it has built-in high pass filters.

If you'd like to experiment on your own, I recommended first reproducing this testing setup ,  then shift slowly towards your application needs. For example you may take the C code and port it to PIC's C18/C30 or AVR-GCC (it shouldn't be too dificult).

Below are some useful resources and their descriptions.

SerialChart executables can be downloaded from here:

SerialChart_01.zip

Once you start SerialChart application you will need to load the imu_arduino.scc configuration file for this project(included in the imu_arduino.zip) archive.

In this configuration file make sure to update the 'port' settings to Arduino's COM port. On my computer Arduino was detected on COM3, on yours it might be different.

For more information on configuration file syntax see:

http://code.google.com/p/serialchart/wiki/ConfigurationFileSyntax

You can also download and compile SerialChart from Google Code:

http://code.google.com/p/serialchart/source/browse/#svn/trunk

You will need a SVN client to checkout the code (I use RapidSVN for Windows).

SerialChart was developed using  Qt SDK from Nokia: http://qt.nokia.com/downloads

 

UPDATE 2010-03-18

Many people ask me what about the other 2 axis, here is the code that outputs 3 axis, including the SerialChart configuration script.

Imu_Arduino_3axis_output_2010-03-18.zip

I also removed some overhead code that Alex pointed out in the comments, this reduced the interval between samples.

In the example below I rotate the board around the X axis(blue) which is parallel to the ground.I do it by hand so X is not exactly 0, but close. The axes that change are Y(red) and Z(green).  Please note the relationship X^2+Y^2+Z^2 = 1. The dashed  cyan, magenta and lime lines are unfiltered signals coming from accelerometer alone (RwAcc).

imu_kalman_arduino_3_axis_captured.png

 

//starlino//

Tags:

Accelerometer, Gyroscope, IMU, Qt, Projects

You May Also Like:

Arduino code for simplified Kalman filter. Using a 5DOF IMU (accelerometer and gyroscope combo) Quadcopter Prototype using Acc_Gyro and a PIC
Arduino code for simplified Kalman filter. Using a 5DOF IMU (accelerometer and gyroscope combo) New and Updated Motion USB Gamepad: wide accelerometer and gyroscope support, configuration utility software
Arduino code for simplified Kalman filter. Using a 5DOF IMU (accelerometer and gyroscope combo) Upgrade your RC Transmitter with a DIY Tilt Motion Control Module
Arduino code for simplified Kalman filter. Using a 5DOF IMU (accelerometer and gyroscope combo) Play PC games the iPad style - Using a PIC with USB, accelerometer and optional Gyroscope
Arduino code for simplified Kalman filter. Using a 5DOF IMU (accelerometer and gyroscope combo) Accelerometer Controlled Usb Gamepad and Mouse using PIC18F2550 / PIC18F4550

Comments/Updates:

Add Comment
Beau (2010-02-09 ):
I really like the Serial Chart application but need some help with the configuration. Could you post a little more on how you configure it?
Starlino (2010-02-09 ):
I added a Wiki page on Google Code project page describing the SerialChart configuration file syntax:

http://code.google.com/p/serialchart/wiki/ConfigurationFileSyntax

Home this helps.
rob (2010-02-11 ):
This is truely a very informative article about this subject. Thank you. I don't get something though: Is it right that you only read one axis? I would like to read both x and y axis inclination and think this should be possible with the set up.
Starlino (2010-02-11 ):
Rob: sure you can add the other axis to the output, update the loop() function as follows:

getEstimatedInclination();
Serial.print(interval); //microseconds since last sample, please note that printing more data will increase interval
Serial.print(",");
Serial.print(RwAcc[0]); //Inclination X axis (as measured by accelerometer)
Serial.print(",");
Serial.print(RwEst[0]); //Inclination X axis (estimated / filtered)

Serial.print(",");
Serial.print(RwAcc[1]); //Inclination Y axis (as measured by accelerometer)
Serial.print(",");
Serial.print(RwEst[1]); //Inclination Y axis (estimated / filtered)

Serial.println("");

And , then in the serial chart configuration define the additional columns:

....

[interval]
color=transparent

[RwAccX]
color=blue

[RwEstX]
color=red


[RwAccY]
color=cyan

[RwEstY]
color=magenta

Keith (2010-02-19 ):
I just got my IMU from Gaget Gangster...and looking to start experimenting with my arduino!

I have a question. My first question was answered to your reply to Rob, but how would I grab the z axis information, and more so filter it using the gyro?...Is there an output for GZ4??

The reason I'm asking; I'm attempting to build a remote head tracking system for first person video in RC planes...I know almost nothing about electronics, but the arduino is making the learning curve much shallower! Ive learned a lot in the past couple of weeks...my background is coding, so i had a head start there...

I origionally started with the memsic 2125 acc. and got all I could with that 2 axis acc...i quicly relaized i NEEDED the z axis for the lateral head movement (shake your head no axis :D )...

So How do i get and filter the z axis on this thing?!? hehe!

Any help would be most appreciated...

Keith
Starlino (2010-02-20 ):
Keith,

the device described in this article is otherwise know as an inclinometer, it measures inclination of the device relative to the ground plane. An IMU unit can have many uses - this is just one of them.

An inclinometer will sense if you tilt the device left/right or forward/backword, but it will not give you absolute information as to the heading of the device North/South/East/West.

You can get some information regarding the rotation around the gravitation vector (the line perpendicular to earth's plane) using the gyroscopes, but gyroscopes only give you relative information, in other words it will tell the speed of rotation, you can integrate these results over a period of time , but without a fixed reference (for N/S/E/W) you will accumulate error over time.

If you absolutely need your device to react to rotation around gravitation vector, you would need to invite a magnetometer to the party. It will provide you a rough reference point from time to time where your device is heading (N/S/E/W), and then you can use gyro's for fine tuning just like you do for accelerometer+gyro combo.

Acc_Gyro is a 5DOF unit, it has a 3-axis accelerometer and a 2-axis gyro (that's why there are no GZ outputs). This is enough to implement an inclinometer. For the "ultimate" IMU device , that would also tell you if you're headed N/S/E/W , you will need 2 more sensor: one more gyro for Z axis and a 3-axis magnetometer.

If you're not ready for more investments, you can still use an inclinometer to control your plane - you should just avoid the rotation around the gravitation vector axis (line then goes from zenith down , perpendicular to earth's plane).
If you're standing up try to define your neutral point not the position where the head is in upward position, but the position where you head is tilted forward (or upward) 45 degrees. This way when you tilt your head you will no longer rotate around gravitation vector, ideally to make maximum use of the gravitation vector as a reference, you would be lying face down, or face up, this way inclinometer would sense every movement of the head with maximum resolution. So getting into an Avatar is still possible with an Acc_Gyro inclinometer , you just have to lie down , just like in the movie :)

If you can't move the body in this position, then move your device - instead of a head mounted screen , consider a hand-held LCD screen that is hold in horizontal position and tilted to control the plane. This might kill some thrill of the project , i.e. the sensation of flying, but there are cost benefits to consider, and maybe also safety since you can still see what's going on around you. This might also be an intermediary stage of your project, and when you mastered it you can move on to incorporating more sensors (magnetometer and extra gyro axis).
Keith (2010-02-25 ):
Starlino,

After a couple of days of messing around with your 5dofImu. I cant seem to reproduce your results in the serial chart program. (I really have to toss it around to get a reading)...moreover, tilting the device on either the X or Y Axis doesnt produce a definitive difference in data...if i tilt the device 45 degrees the serial data might change .01... can you shed some light on what i may be doing wrong.

RwAcc[0], RwEst[0]
-0.58,-0.47
-0.58,-0.50
-0.58,-0.53
-0.58,-0.57
-0.58,-0.60
-0.58,-0.63
-0.58,-0.64
-0.58,-0.66
-0.58,-0.67
-0.58,-0.69
-0.58,-0.70
-0.58,-0.70

My goal is to produce a device similar to:
http://vimeo.com/2590122

Im not looking to control the plane's heading, just to control a camera on board...by moving my head, the camera will move...a UAV is the 20th step for me...

I just need to harness that "Yaw" movement, and I dont know enough to make it happen.

Thank you for any help!
Starlino (2010-02-25 ):
Keith,
can you please also output and post the raw analog readings values an[0]..an[5], while holding device in stable horizontal position.

Also it makes sense to check you ACC_GYRO outputs with a multimeter, voltages you should check are: AX,AY,AZ, GX4, GY4

Please post the results here and we'll go from there.


Keith (2010-02-25 ):
ok, here are the analog readings:
an[0]: 28, an[1]: 26, an[2]: 27, an[3]: 254, an[4]: 309, an[5]: 256
an[0]: 28, an[1]: 26, an[2]: 27, an[3]: 253, an[4]: 329, an[5]: 256
an[0]: 27, an[1]: 26, an[2]: 27, an[3]: 254, an[4]: 287, an[5]: 256
an[0]: 27, an[1]: 27, an[2]: 27, an[3]: 255, an[4]: 246, an[5]: 256
an[0]: 27, an[1]: 27, an[2]: 27, an[3]: 252, an[4]: 272, an[5]: 256
an[0]: 28, an[1]: 27, an[2]: 27, an[3]: 252, an[4]: 231, an[5]: 256
an[0]: 28, an[1]: 26, an[2]: 27, an[3]: 253, an[4]: 186, an[5]: 256
an[0]: 27, an[1]: 27, an[2]: 26, an[3]: 253, an[4]: 195, an[5]: 256
an[0]: 27, an[1]: 27, an[2]: 26, an[3]: 251, an[4]: 191, an[5]: 256
an[0]: 28, an[1]: 26, an[2]: 26, an[3]: 252, an[4]: 177, an[5]: 256
an[0]: 27, an[1]: 26, an[2]: 26, an[3]: 251, an[4]: 197, an[5]: 256
an[0]: 27, an[1]: 27, an[2]: 26, an[3]: 253, an[4]: 239, an[5]: 256
an[0]: 28, an[1]: 26, an[2]: 25, an[3]: 252, an[4]: 332, an[5]: 256
an[0]: 27, an[1]: 26, an[2]: 26, an[3]: 254, an[4]: 309, an[5]: 256
an[0]: 27, an[1]: 25, an[2]: 26, an[3]: 252, an[4]: 323, an[5]: 256
an[0]: 27, an[1]: 26, an[2]: 26, an[3]: 253, an[4]: 320, an[5]: 256
an[0]: 27, an[1]: 26, an[2]: 25, an[3]: 255, an[4]: 304, an[5]: 256
an[0]: 27, an[1]: 26, an[2]: 25, an[3]: 254, an[4]: 277, an[5]: 256
an[0]: 26, an[1]: 26, an[2]: 26, an[3]: 252, an[4]: 307, an[5]: 256
an[0]: 27, an[1]: 25, an[2]: 26, an[3]: 254, an[4]: 254, an[5]: 256
an[0]: 27, an[1]: 25, an[2]: 26, an[3]: 252, an[4]: 209, an[5]: 256
an[0]: 27, an[1]: 26, an[2]: 25, an[3]: 250, an[4]: 210, an[5]: 256

here are the 3 axis readings:

RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.57, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.52
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.52
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.53
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.52
RwAccX: -0.58, RwEstX: -0.57, RwAccY: -0.58, RwEstY: -0.63, RwAccZ: -0.58, RwEstZ: -0.52

My multimeter isnt working sorry...

can you tell anything from what I gave you?

Thanks! Keith
Starlino (2010-02-25 ):
Keith,

It looks like accelerometer values are way off ,
an[0] (AX), an[1] (AY) should be around 1.65V / 5V * 1023 = 337.59

When you get a multimeter, please remove the Acc_Gyro from your project power it with 5V power source on 5V and GND pins. Then measure the voltage between GND and AX, AY. When Acc_Gyro is in horizontal position they should be around 1.65V.



Alex (2010-03-05 ):
Your serial chart application is a must !
It did not manage to use the port COM18 of the Arduino I use. After some tweaking in Windows, I changed it to port COM2 and it works very fine. I'm sure it will help me a lot to develop a specific IMU for my tricopter, as well as your simplified Kalman filter code.

I noticed a small mistake in your code, with no consequence in the computation result:
//reverse calculation of RwGyro from Awz angles, for formulas deductions see http://starlino.com/imu_guide.html
for(w=0;w<=1;w++){
RwGyro[0] = sin(Awz[0] * PI / 180);
RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
RwGyro[1] = sin(Awz[1] * PI / 180);
RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
}
I suppose you wanted to mutualize the code in the loop, but this way it is done twice.

I also noticed the sensibility scaling of the ACC is useless in your formulas if it is the same on the 3 axis because of the vector normalization.
Lucas (2010-03-18 ):
Hello, Starlino first of all congratulations for your work and for share it with the world. I implemented this code and noticed some that I want to ask you. It might be doubt of others readers too. With this code when you are monitoring only one angle (Roll or Pitch) it works perfectly, like the picture previously shown. But when you capture the two angles (Pitch and Roll) at the same time, the rotation of one influences a lot on the estimation of the other.

Let me be more clear, for instance if I rotate my IMU only over the X axis, I see the Pitch angle (formed by axis Y and the gravity vector) angle changing perfectly of 0º to 180º, but the Roll angle (wich was supposed to stay in zero all time) changes too. I believe its a effect of the filter but I didin't found this relation. Can you give a tip of how change this variation or minimize her?

Another question. Do you know a method to put this angle variation of 0º to 360º?

Thank you.
Starlino (2010-03-18 ):
Lucas,
if you rotate the board around one axis for example (X) , only the other 2 axes should change. I included code and a screen shot in the article update "2010-03-18", see above (You are right many people ask same question).

You can get a 360 degree angle (around one of the axis, for example X) by using atan2(RyEst,RzEst), it will return an angle between -PI to PI in radians (or -180 to 180 in degrees).

If you can't reproduce this exactly with your sensors, I recommend to start the Acc_Gyro sensor then migrate the code to your sensors.
Ozan Enginoglu (2010-04-06 ):
Why did we get the reference voltage as 5 V? Because in the datasheets of the accelerator and gyroscope they both get about 3.3 V voltage input. Shouldn't we assign reference voltages according to datasheet input voltages?
Starlino (2010-04-06 ):
Ozan ,
we use 5V because this is the reference voltage of the ADC module of Arduino. In other words 5V will correspond to a maximum value of 1023 that can be theoretically returned by the analog read. The Acc_Gyro will obviously never go over 3.3V so the remaining range of ADC (up to 5V) is simply unused.

On the UsbThumb(it's a PIC platform that mates with Acc_Gyro) on the other hand the entire range of of ADC is used because it is powered by 3.3V.


DJ (2010-04-24 ):
This write-up is absolutely brilliant. I have never built/programmed an Arduino (or any PCB) device before. However, I was able to read these instructions, buy the parts and put it all together.

With that said, it looks like you take what would have been an analog output and convert it to digital to then provide a readable serial output. I would like to output an analog (or PWM) 0.0 - 5v (range 0 - 45 or 90 degree tilt) signal that I can feed into another device.

Would that be easy with this code? Is there somewhere I could read up more on this to find out what in your code needs to be modified to do this?
ilco (2010-04-26 ):
I really like this article and some others (about the theory of IMU, programming & soldering tips for PIC by instance).

I have tested your code with the IMU and a dsPIC33FJ128GP802. I'm quite impressed by the kalman filter! Without your code I thought I had no chances to program this filter in less than few months (I try to use the "simple" PID filter of the dsPIC33F). It works great, however since it is for an UAV (Quadcopter) I think that the lack of Gyro drift compensation might be annoying (I haven't tested in flight for that reason :D ).

Your article about the IMU deals with combining gyro and accelerometer datas, but I haven't found something about the drift!

I'am also thinking about a 3rd axe for the gyro (soldering a new gyro on a separate board), but it might be useless...

So if you have got informations about the drift compensation or the 3rd axe, it would be kind to help me.

Best regards

Starlino (2010-04-26 ):
Ilco:

About the 3rd gyro axe - you'll need to stabilize yaw rotation. They are cheap , Pololu has one for 17.95 http://www.pololu.com/catalog/product/765 .

There are 2 types of gyro drift often confused, one is that results from integrating the gyro readings over a period of long time in gyro-only systems. This is not the case with the algorithm described in this article since gyro is only integrated over a short period of time (older values simply fade out as they are combined with accelerometer readings).

The other type of gyro drift is caused by the fact that gyro outputs a zero-point that differs from the specs, sometimes it even changes over time (not with Acc_Gyro that has a low pass filter that takes care of this). The Acc_Gyro is pretty stable in this respect, so if it's spec zero-point is 1.23V you can expect an actual zero-value in range of 1.22..1.24. A module I have from Sparkfun on the other hand goes all the way to 1.11V, I guess it's the difference between the reflow process that causes these differences with same sensors. Long story short, don't trust the specs - before you start your device you should calibrate the zero-values of your gyro. For example ask the user not to move the device on start-up, blink a led for status, monitor for gyro values being relatively stable and the read the actual zero-value (mid value not average is recommended). You can store these values in EEPROM for future use. With Acc_Gyro I haven't seen much variation over time or temperature. So if you determine your Acc_Gyro gyro's zero value to be for example 1.239 ,it will be like this even after a month or two.

I think the real problem you'll have with a quadcopter is the noise from the motors. I am working on a similar design (also based on dsPIC33FJ128GP802), and I am planing to solve this by taking more ADC samples than is possible with Arduino, using PIC's DMA access to fill a memory location , then average them every 3 ms and feed the average values to the algorithm in this article. The algorithm itself has a property of smoothing the inclination readings as shown on the SeriaChart screenshot, but I think the increased sample rate and averaging that is possible to do with the PIC will help even more (it's like having a separate thread in the background collecting ADC samples , while your main code thread is doing something else). If you want I can share the ADC code for DMA readings on dsPIC33FJ128GP802. I'll probably have more info on other issues as project progresses, also since we're using same chip I am open to cooperate on the project with you or other people.
ultramagnus (2010-07-24 ):
Thank you very much for this, it helped me greatly understand how a kalman filter works, your code is very well commented and very clean.
© Starlino // If you reproduce content from this site please give credit to the author // Contact

Designed By Starlino:

USBThumb_Propeller_Programmer.jpg
UsbThumb (Propeller Programmer & more)

acc_gyro.jpg
Accelerometer+Gyro
5DOF IMU 

motion_gamepad.jpg
Motion Gamepad Kit

125x125PcbwingBanner.gif

Cheap quality PCB fab.
Use link above & coupon
code STARLINO for
extra discount !

Follow me on:

twitter_logo_header.png

youtube.png

RSS FEED RSS FEED

 

Cool Sites:

GadgetGangster.com
Electronics-Lab.com
CuriousInventor.com
LetsMakeRobots.com
Eevblog.com
Screamingcircuits.com
Hackaday.com
Embedds.com
Hackedgadgets.com


See me on Make!

UsbThumbImu


See me on Make!

Usb Gamepad

See me on Make!

Reverse Surface Mounting