Initial implementation of a 9 DOM/DOF MARG IMU orientation filter with ADXL345, ITG3200 and HMC5843 on Arduino

Last updated on Tue, 2011-05-10 13:49. Originally submitted by fabio on 2010-12-04 16:15.

UPDATE 2011-05-10: The 9 DOM sensor fusion library presented in this article is now part of the FreeIMU library. Please consider the code in this page as outdated and just use the FreeIMU library.

I spent the last days creating an initial implementation of a 9 Degrees of Measurement (DOM) / Degrees of Freedom (DOF) AHRS sensor fusion orientation filter. I've created a library, called FreeIMU, which polls data from the ADXL345 accelerometer, the ITG3200 gyroscope and the HMC5843. Have a look at the attachments for my circuit schematics.

To access the ADXL345 accelerometer I used this nice Arduino library (based on this other one), while for the ITG3200 I used Filipe Vieira's ITG3200 Arduino library. To access the HMC5843 I used my library.

The sensor values are then pushed into Sebastian Madgwick's implementation of Mayhony's DCM filter incorporating Sebastian's magnetic distortion compensation (Algorithm available here - project here).

IMPORTANT: This code doesn't disable the ATMEGA internal pullups so a logic level converter/translator is needed if you don't want to fry your sensors. In the video I used a modified version of twi.c (inside of the Wire library) commenting out the pullup enabling in twi_init().

The result can be seen in the video below. Note that all the code published here is still very very young. You'll probably find lots of bugs and WTF but I think it can still be useful. Keep an eye on this website for the next developments. If you are working on this stuff get in touch with me if you want to share efforts.

AttachmentSize
FreeIMU breadboard Schematics11.86 KB
Arduino Code for 9DOM sensor orientation filter31.11 KB
Processing Code for 9DOM sensor orientation filter36.88 KB

Using 2 sets of SEN-10724

Submitted by Mary (not verified) on Thu, 2013-06-13 09:03.

Hi, i was just wondering if anybody knows, Im doing a project involving sen-10724 as well using arduino software. I was just wondering if anybody happen to know let say i put 2 of these sensors from 1 end to another end in a bendable stylet for example, Knowing that i have the angle of curvature already, how do i calculate the distance of this curvature between this 2 sensors at the point when i bend it? anybody have an idea?

implementation of a 9 DOM/DOF on Arduino using different sensors

Submitted by Andresc4 (not verified) on Tue, 2013-03-26 20:40.

Hey this website its great, added to my bookmarks!
I found this site looking for a kalman Filter in arduino for a 9dof
i have a GY85 IMU and i would love to implement this code to this board, but to be honest, im kind of newbie in how to do it. So, 1st question, is it possible ? or some calculations are done in the chip of the Freeimu ?

this is what im doing now http://www.youtube.com/watch?v=_bVDa_oMN2E&list=UUSd-iuYAFI09VOXj1HCHgFA...

but i need to get a more presice location like you are doing.

Thanks a lot, Andres

How to translate 3D object

Submitted by Art (not verified) on Sat, 2013-03-02 13:32.

How to translate 3D object according to IMU sensor movement in real-time?

How to translate 3D object

Submitted by Art (not verified) on Sat, 2013-03-02 13:32.

How to translate 3D object according to IMU sensor movement in real-time?

6DOF Sensor Fusion with Wii Motion plus and Nunchuk

Submitted by sajid (not verified) on Sat, 2012-09-01 06:52.

hi ,
i have done the 6DOF Sensor Fusion with Wii Motion plus and Nunchuk by modifying the freeimu library for mp and nunchuk :), here is my blog
http://engrsajidzaman.blogspot.com/ video:

How get +-(left/right) value from accelerometer

Submitted by flimbo (not verified) on Sun, 2012-08-05 17:39.

Hi there,
i have 9DOF from Sparkfun and using AHRS Razor code.
but i have trouble how i can get negative values for x movement ?
i mean when i move device left along x asis it should give me negative reading ? coz it only give me positive values.

Experiencing drift in all directions....

Submitted by Bob F. (not verified) on Wed, 2012-06-20 02:48.

Hello,

I am new to IMU and using a SEN-10724 9 Degrees of Freedom sensor. I am currently experiencing large drifts in the output after just 2 minutes.

The sensor is flat and stationary for the entire two-minute test. The outputs of the Gyro, Accel, and Mag are as follows:

G: -14, -18, -5
A: 5, -2, 259
M: 184, -223, -119

These values remain more-or-less the same (+/- 3) for the duration of the test. But the outputs (in degrees) for Pitch, Roll, and Yaw start at ...

Pitch: 0, Roll: 0, Yaw: 0

and end at ...

Pitch: 37, Roll: -7, Yaw: -32

after 4 minutes, they are:

Pitch: 46, Roll: -9, Yaw: -165

I am using a bias calibration with multipe samples similar to this: https://sites.google.com/site/innovationinx/software

After adjusting for bias and converting the gyro values ro radians, all values that are actually sent to the MadgwickAHRSupdate() function are fractional. i.e. 0,-0.001, 0.001, 0.018, 0.011, 0.039, 0.007, -0.617, 0.007

What am I doing wrong? I would think that the Magnetic sensor fusion would at least keep the yaw from drifting.

Thanks.

-Bob F.

questions about IMU...

Submitted by Sergio (not verified) on Tue, 2012-05-22 15:53.

Hello Fabio:
We would like to ask you about your software and its influence in basics concepts of navigation:
-Could you explain briefly advantages to use magnetometers? Is it strictly necessary?.
-Could we use this IMU in an environment of iron? E.g. inside the car.
-Is your software library usable no matter the relative ubication between the three sensors?
thanks,
Sergio.

The sensor fusion algorithm

Submitted by fabio on Tue, 2012-05-22 16:04.

The sensor fusion algorithm implemented here uses the gyroscope as main source of orientation. Unfortunately, gyroscopes are subject to drifting thus after some time the orientation estimated drifts away from the actual one. This is fixed by fusing the gyroscope with the accelerometer, which compensates from drifting on the pitch and roll angles, and the magnetometer which compensates from drifting on the yaw. Basically, the idea is to add the gravity and magnetic north as additional point of reference to the attitude and heading estimation.

Please refer to the FreeIMU libraray for the most updated code.

Forgot the other 2

Submitted by fabio on Tue, 2012-05-22 16:12.

Forgot the other 2 questions..

Yes, you can use the software also with iron nearby.. you will probably have to calibrate it, but in general it should be fine. Problems are varying electro-magnetic disturbances which can't be calibrated out. I suggest you some prototipization to see if that's acceptable for you.

You can use the code with sensors placed in whichever way you want. The closer the sensors are, the best results you get. In case sensors axis aren't aligned, you'll have to align them later in software (in the FreeIMU library there are examples of that for the Sparkfun's IMUs which don't have their axis aligned).

Hi guys I want ask you....at

Submitted by Alessandro Zupo (not verified) on Sun, 2012-03-18 20:40.

Hi guys

I want ask you....at this function:

AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)

What value i had to send at the function alghoritm if i don't have megnetometer??

Thanks a lot.

Use the FreeIMU library, it

Submitted by fabio on Sun, 2012-03-18 22:13.

Use the FreeIMU library, it now has a compilation flag for using it with 6DOF boards without magnetometer.

AHRS filter wihtout Magnetometer

Submitted by Alessandro Zupo (not verified) on Mon, 2012-03-19 13:58.

Hi , i'm not using Arduino, i use a my PIC18f2550 development board. I would like to see the code of AHRS filter without magnetometer. Where can i see??

Thank you very much.

Click on the link in the

Submitted by fabio on Mon, 2012-03-19 14:02.

Click on the link in the above comment, you'll see some libraries for download. Download them then look for FreeIMU.cpp .. in there, the function you need is called FreeIMU::AHRSupdate.

Update Filter problem

Submitted by Alessandro Zupo (not verified) on Wed, 2012-03-21 23:57.

Hi Fabio,

i was able to implement you alghoritm in Vb.2010

Now the problem is that.....from the start acquisition the 3Dcube start to rotate and accumulate an error.....why?? what is wrong??

That's impossible to say with

Submitted by fabio on Thu, 2012-03-22 14:11.

That's impossible to say with so few information.

Ok i found It.

Submitted by Alessandro Zupo (not verified) on Mon, 2012-03-19 14:31.

Ok Fabio, now i have found it : - )
I will do some test.

Thank you.

Roll and Pitch swapped

Submitted by Daedelus (not verified) on Mon, 2011-12-12 17:28.

First, thanks for the FreeIMU library. I have been able to get a much better result with it than anything I have tried so far.

I am using the Sparkfun SEN-10724 and a Duemilanove. I am a programmer with some electronics way back in my resume.

I have two questions about the display output. One is that in the block display, roll and pitch are swapped. Can you point me to what I need to change in the code?

And while the yaw seems to move as it should, after a move, the block slowly returns to the home position. Can you tell me if this is normal?

As the smart guys at Sparkfun

Submitted by fabio on Mon, 2011-12-12 21:55.

As the smart guys at Sparkfun didn't align the three sensors axis on their board, while my board does have them aligned, you have align them in software. See older comments here and on FreeIMU project page. Once you align them, it should work as expected.

Please consider the code here as outdated. Please use the FreeIMU library.

I'd love to be using your

Submitted by Daedelus (not verified) on Fri, 2011-12-16 20:51.

I'd love to be using your board, but they don't seem to be available.

We are working on that ;-)

Submitted by fabio on Sat, 2011-12-17 21:46.

We are working on that ;-)

Thanks, I did sign up to be

Submitted by Daedelus (not verified) on Sun, 2011-12-18 00:53.

Thanks, I did sign up to be notified. And BTW, I did get the SEN-10724 working using the Razor_AHRS firmware. I had to change the SENSOR_SIGN array and swap the magnetom[0] and magnetom[1] buffer addresses. It works OK now but I am very interested in the Human Body Tracking project by John Patillo so I'll be following that as it develops.

My project using your code has been featured to Instructables.

Submitted by Kaeru no Ojisan (not verified) on Mon, 2011-10-17 14:26.

Hi, Fabio!

My project using your code has been featured to the Instructables.com homepage and selected to be on the Front Page.

I have modified your code for ITG-3200 only and added lines for communication from Processing to Arduino sending initial phi angle.

Thank you very much.

Awesome! What's the link/name

Submitted by fabio on Mon, 2011-10-17 14:51.

Awesome! What's the link/name of the project? I'm not finding it..

Car No. 06 Programmed Automatic Driving Car

Submitted by Kaeru no Ojisan (not verified) on Mon, 2011-10-17 15:01.

Title of my project is "Car No. 06 Programmed Automatic Driving Car".

http://www.instructables.com/id/Car-No-06-Programmed-Automatic-Driving-Car/

But already disappeared from the front page.

get yaw pitch roll

Submitted by Anonymous (not verified) on Thu, 2011-10-13 18:20.

can someone tell me what is the difference between the two functions ,getYawPitchRoll() and getEuler() ?

getYawPitchRoll() computes

Submitted by fabio on Thu, 2011-10-13 22:42.

getYawPitchRoll() computes the angle between the heart plane and the X (pitch) and Y (roll) axis of the IMU while getEulers returns Euler angles thus a set of three consequent rotations.

Hi Fabio, i want ask

Submitted by Alessandro zupo (not verified) on Thu, 2012-03-22 19:52.

Hi Fabio, i want ask you...
When you execute the update filter function you pass (gx,gy,gz) direct gyroscope values read directly from Itg3200 and convert in radians or the angle returned from the other function getYawPitchRoll() ???

gyroscope readings in rad/s.

Submitted by fabio on Thu, 2012-03-22 20:25.

gyroscope readings in rad/s.

Maybe axis problem

Submitted by Alessandro Zupo (not verified) on Thu, 2012-03-22 22:10.

Hi Fabio,

maybe i have some problem with axis allignment of Gyro and Acc.

The problem is that, my Cubo start to move if my pcb is lock too.

What happen if you start the acquisition with the PCB rotate of 180 ° in Z. That Can be a problem??

Thank you.

quadcopter

Submitted by Anonymous (not verified) on Wed, 2011-09-28 11:27.

I'm using this code in a quadcopter, the problem is that the angle is always changing like 0.5 more or less, and it is bad for stability..there is some way to put this more precise?

Sample period

Submitted by Anonymous (not verified) on Mon, 2011-09-26 15:33.

Hi guys, can someone tell me what is the sampling period and where can i change it?

i'm talking about the

Submitted by Anonymous (not verified) on Mon, 2011-09-26 15:34.

i'm talking about the sampling period of the filter...

If you are using this code

Submitted by fabio on Mon, 2011-09-26 17:00.

If you are using this code you have to change both PERIOD in the .pde file (period expressed in milliseconds) as well as halfT (half of the pediod expressed in seconds) in FreeIMU.cpp.

I would suggest using the new FreeIMU library which will adjust it automatically to as many calls to AHRSupdate you do.

Yap, the problem is that i

Submitted by Anonymous (not verified) on Wed, 2011-09-28 11:25.

Yap, the problem is that i wanna know what is the sample time, and with the new code the time is the time that the function takes..or am i wrong?

see in real time

Submitted by Lenin (not verified) on Sat, 2011-09-10 18:52.

Hi fabio, What software I can use to see the movement in real time in the pc?

You have to use Processing.

Submitted by fabio on Mon, 2011-09-12 09:50.

You have to use Processing. The source code for it are in the attachments to this page.

Calibrate

Submitted by Anonymous (not verified) on Fri, 2011-09-09 15:40.

Hi Fabio,
I'm using this library, and everything is working. but i have one problem.
When i have my IMU in stationary position, i'm reading the pitch angle correctly, but my roll angle is reading something about -7 degrees..there is some function to calibrate the angles ?

someone can help me?

Submitted by Anonymous (not verified) on Mon, 2011-09-12 11:29.

someone can help me?

Few notes

Submitted by Paolo Mosna (not verified) on Mon, 2011-09-12 13:43.

Quaternion estimation is subject to two errors (mainly) one error is due to (1) sensors calibration - the gyro for example give out offset values when stationary and moreover the compass doesn't have uniform values for each axes so for example maximum value on X is 500 and on z maximum value is 350; the same is true for the accelerometer - and to (2) PCB positioning errors - it is very difficult to perfectly align each single sensor one respect to the other when mounting sensors on PCB.

If - for one moment - we suppose aforementioned errors not present, the zero position is the position in which you read all angles to zero. So suppose X axes points toward nord and device is perfectly horizontal.

If - in this position - you do not read all angles to zero, you have to compensate for calibration and for sensor's positioning errors.

Here http://www.mathworks.com/matlabcentral/fileexchange/23398-magnetometers-... you can find a matlab function to calibrate magnetometer. You can run the function with octave if you can not afford using metlab. I used same procedure to calibrate also the accelerometer.

To solve PCB sensors alligment problem I used rotation matrix to correct those errors before passing data to data fusionn algorithm.

Hope this help you.
Paolo.

thank you, i didn't try yet,

Submitted by Anonymous (not verified) on Tue, 2011-09-13 00:58.

thank you, i didn't try yet, but tomorrow i will do it.

Check that the mounting of

Submitted by fabio on Mon, 2011-09-12 11:46.

Check that the mounting of your IMU is perfectly plane. If it is, you may need to calibrate your accelerometer. See http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/... for examples on how to do so (needs programming experience and some math knowledge).

If you use the latest FreeIMU

Submitted by fabio on Mon, 2011-09-12 11:56.

If you use the latest FreeIMU library, you can use the getYawPitchRoll method to get those angles and simply remove the -7 from the roll angle (it won't work with the code in this page as they are Euler angles, thus correlated).

I used that function yaw

Submitted by Anonymous (not verified) on Mon, 2011-09-12 12:09.

I used that function yaw pitch roll..so..are you saying read the angle and remove the error with a minus operation? lIke my roll angle is always Roll-7...

Nope sorry.. I was over

Submitted by fabio on Mon, 2011-09-12 14:02.

Nope sorry.. I was over simplifying the problem.. a proper calibration may be needed.

Algorithm

Submitted by Anonymous (not verified) on Fri, 2011-09-02 18:38.

Hi Fabio,
i have one debt about this algorithm Quaternion implementation of the 'DCM filter' ...so i was reading Madgwick's thesis and he has a MARG sensor algorithm to magnetic distortion and gyroscope drift..I wanna ask you if in this FreeImu Implementation you have the Gyroscope drift too?because i was reading your thesis too, but you speak just about magnetic distortion compensation..
I hope you can help me i'm a bit confuse

The algorithm you find here

Submitted by fabio on Fri, 2011-09-02 19:18.

The algorithm you find here has been also implemented by Sebastian. This code does include gyroscope drifting compensation by means of gravity and magnetic vector observations.

I was testing your code and

Submitted by Alunmi (not verified) on Tue, 2011-07-26 15:51.

I was testing your code and it works nice, but sometimes the processing Cube Crashes.i noted that the Arduino process still works, only the processing crashes.But then i tested to get the Euler angles in the Arduino code and the problem still the same, i started to get ZERO instead values of euler angles...

I already tested the wires.
Some ideas?

Would you mind trying with

Submitted by fabio on Tue, 2011-07-26 16:58.

Would you mind trying with the updated library?

Post new comment

The content of this field is kept private and will not be shown publicly.
If you have a personal or company website insert its address in the form http://www.example.com/ .
  • Web page addresses and e-mail addresses turn into links automatically.
  • Allowed HTML tags: <a> <em> <strong> <cite> <code> <ul> <ol> <li> <dl> <dt> <dd> <pre> <small> <del> <img> <h2> <h3> <h4> <b> <video> <sub> <sup>
  • Lines and paragraphs break automatically.
  • Images can be added to this post.
  • You may use [inline:xx] tags to display uploaded files or images inline.
  • You may insert videos with [video:URL]
  • Each email address will be obfuscated in a human readable fashion or (if JavaScript is enabled) replaced with a spamproof clickable link.

More information about formatting options