Go Down

Topic: Guide to gyro and accelerometer with Arduino including Kalman filtering (Read 776774 times) previous topic - next topic


No if you only need to know the heading while the magnetomer is in a horizontal position, that is all you need!
But if you are going to tilt it or if it's not perfectly flat, you have to "tilt compensate" it using a accelerometer (and a gyro). You can use both a accelerometer and a gyro to get better results, as you can then use the output from the Kalman filter.

See this guide for more information: https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial



Yeah I saw that.

I've got all 3 (accel/gyro/mag), so that's the direction I'm going.

This would be ideal:

The academic paper is HARD to read through, and the straight code isn't directly compatible since I'm doing everything in (computer) software, so it's a matter of extracting the math from the code/paper and applying it elsewhere.

For the moment I have 6 streams (accel/gyro) coming in nicely calibrated (thanks to your guide), but in the end I just watch the absolute angles.

Speaking of that, when you do sensor fusion (like your kalman code, or this marg example) what is the end result?

So with kalman filter you get 3 values (pitch/roll/yaw?) but more stable due to the fusion?
If you throw in the magnetometer do you end up with 3 values at the end also, but super stable and non drifting? Or does it produce quaternions?


You can use his C# implementation of his code: http://www.x-io.co.uk/node/8#open_source_imu_and_ahrs_algorithms. If you are not running Windows, then see: http://monodevelop.com/ which will allow you to build it on Linux and Mac OS X as well.

The problem when using euler angles is that you wouldn't get any precise measurements, when you tilt the device too much, try for instance to tilt the IMU 90 degrees for instance at pitch, you can immediately see that roll doesn't make sense anymore as it's suddenly yaw (this is called gimbal lock: http://en.wikipedia.org/wiki/Gimbal_lock).
So instead of using euler angles you have to use something called quaternions, I won't try to explain what it is since I have never taken the time to read through the theory, as I have never needed to know the exact position of all the axis, as I have always used the Kalman filter for my Balancing robot: http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/.
You can find more about quaternions in the following video: http://youtu.be/C7JQ7Rpwn2k and in the first link in the post.
Also take a look at: http://en.wikipedia.org/wiki/Gimbal_lock#The_quaternion_solution and of course: http://en.wikipedia.org/wiki/Quaternion

So in short, if you want something like in the video above, you shouldn't be using euler angles.



That google video is great!

So in watching that I think I've come to a couple of conclusions.

The first one is that it might be best to crunch my numbers back on the Arduino. My original thinking in sending out raw data is that I'm more comfortable with programming on the computer (Max/MSP) than I am in Arduino, and it's much easier to test/adapt/modify in that environment. I also thought it would be faster since my computer CPU is significantly faster than an Arduino.

I didn't account for the bottleneck of serial communication through. So all my fusion stuff will happen potentially displaced by serial latency (unless I'm incorrect in thinking that data travelling across a serial pipe is slower than data in a void loop circling around and around).

That's a big change from what I'm doing now, and I'm in the middle of a project based around this so I'm not gonna 'jump ship' just yet (and just use the 6axis data I have).

It also looks like quaternions is in fact what I'm after (and what that code does).

Now it's just a matter of how much of that code can live in Arduino land (and how much is based on their hardware/protocol).

The final thing is that I'm wondering if the way I have my sensors oriented is a problem. You can see my boards here in an old photo when I had first put them together.

I have the magnetometer directly above the MCU. It is pointing in the correct direction, but I don't know if having something directly below it (a couple millis as there's poofy double-sided tape) is a problem.


Actually the comments in the video indicate that what we're seeing is the onboard fusion (on the right) and the raw data being sent to the computer for fusion there and both look good/tight. This is a bluetooth device so I don't know what kind of data rates that has for the raw data. The fastest I've been able to run my XBees is 57600, without it crashing (115200 worked for 2 seconds then stopped).


Aug 16, 2012, 08:07 am Last Edit: Aug 16, 2012, 11:07 pm by Battousai Reason: 1
Hi Lauzus, first of all i would like to thank you for the information you post. I think your post is just what i need for mi project. I need to make a control system for a camera roll-tilt mount, using a 5DOF(ADXL335 and IDG500) IMU and two servomotors. Here is the board i'm using https://www.sparkfun.com/products/9268.

Here are the datasheets from the gyros and the accelerometer.

I have upload your code IMU6DOFVer3 and change the pins configuration to that 5DOF. For my project i don't need to get 360 degrees resolution, so I'm using the part from the previous version to calculate the AccXangle and Yangle ( i mean i'm calculating the force vector and then using acos function).

When i upload your code, the printed results for Xangle are the ones for the Yangle and viceversa. I think it's because the orientation of the IC's (specifically the IDG500). I looked to the 6DOF IMU you use and notices that the gyro x axis from yours  is the gyro y axis of my 5DOF. A similar thing happens with the gyro y axis of your 6DOF. I've changed the pins  Gx for Gy, but i'm still getting the same problem.

I've been stuck in this for 2 days. I really hope you could help me.


Aug 16, 2012, 11:00 pm Last Edit: Aug 16, 2012, 11:10 pm by freak174 Reason: 1

First of all, thank you !

I took a look into ur  code (for I2C) u provided in a reply and put it together with another code.

ZeroValues > I did not take these values from the datasheet, I just ran a script with raw values and took the average values when the sensor was lying on the floor.
GyroSensitivity > I actually dont know which one I should take from the datasheet, but Ive tried everyone (there are 4 levels).

These are the values I get when the sensor is lying on the floor
343.26   136.31   
344.40   126.77   
345.49   117.89   
346.50   109.65   
347.41   101.99   
348.27   94.83   
349.07   113.36   
349.82   105.43   
350.48   98.05   
351.12   91.18   
351.71   84.80   
352.27   104.04   
352.78   96.76   
353.25   89.98   
353.69   83.68   
354.12   77.82   
354.51   72.36   
354.87   67.28   
355.23   87.76   
330.36   106.78   
332.41   99.32   
334.33   92.38   
336.12   85.92   
337.78   79.92   
339.31   74.32   
340.73   69.13   
342.06   64.28   
343.30   59.78   
344.46   55.58   
345.50   76.87   
346.49   96.67   
322.25   115.07   
324.85   107.02   
327.25   124.71   
329.51   115.98   
331.62   107.86   
333.54   100.32      

And here is the code

Code: [Select]
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

bool blinkState = false;

unsigned long timer;

double zeroValue[5] = {950, -400, 13500, -100, -500};

/* All the angles start at 180 degrees */
double gyroXangle = 180;
double gyroYangle = 180;

double compAngleX = 180;
double compAngleY = 180;

void setup() {

   // join I2C bus (I2Cdev library doesn't do this automatically)

   // initialize device
   Serial.println("Initializing I2C devices...");

   // verify connection
   Serial.println("Testing device connections...");
   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  timer = micros();

void loop() {
   // read raw accel/gyro measurements from device
   //accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

   accelgyro.getRotation(&gx, &gy, &gz);
   double gyroXrate = -((gx-zeroValue[3])/131); //Change the sensitivity after your sensor
   gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Without any filter
   double gyroYrate = ((gy-zeroValue[4])/131);
   gyroYangle += gyroYrate*((double)(micros()-timer)/1000000); // Without any filter
   //The acc X and Y angle///
   accelgyro.getAcceleration(&ax, &ay, &az);
   double accXval = ax-zeroValue[0];
   double accZval = ay-zeroValue[2];    
   double accXangle = (atan2(accXval,accZval)+PI)*RAD_TO_DEG;
   double accYval = ay-zeroValue[1];
   accZval = ay-zeroValue[2];    
   double accYangle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG;
   compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle);
   compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);

   timer = micros();
 Serial.print(compAngleY); Serial.print("\t");
 //Serial.print(timer); Serial.print("\t");


Im using a sensor called MPU6050.
Datasheet can be found here:

When I move it like one mm to another direction it seems to be stable, I guess there is a point where everything gets screwd up..
2.51   1.53   
2.49   1.50   
2.48   1.49   
2.48   1.49   
2.47   1.49   
2.47   1.48   
2.46   1.46   
2.47   1.44   
2.49   1.41   
2.51   1.37   
2.52   1.38   
2.54   1.38   
2.51   1.38   
2.51   1.39   
2.50   1.41   
2.48   1.40   
2.46   1.38   
2.48   1.39   
2.49   1.38   
2.51   1.39   
2.50   1.38   
2.51   1.39   
2.50   1.40   
2.49   1.39   
2.50   1.40   
2.49   1.42   
2.46   1.43   
2.45   1.41   
2.44   1.41   
2.47   1.40   
2.44   1.42   
2.46   1.43   
2.47   1.46   
2.44   1.45   
2.44   1.46


I would like to know what his loop rate is, as I don't think he can do much else than printing the values as there is a lot of calculation going on. But you can always try to use a more powerfull device like the mbed (http://mbed.org/). I have used it in the past and I really like them as well.

I don't think it's a problem that you have just stacked the boards. My only concern is that the airflow under the magnetometer is really bad so the heat from the ICs can't get away, but since a gyro or accelerometer doesn't produce much heat I really don't think it's a problem :)

My XBees work just fine at 115200 baud. I use the normal ones with onboard antenna.

Take a look at the newest code at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter. What I normally do is that I just calculate the unfiltered angle using both the gyro and accelerometer so I'm sure that the orientation and sign is the same and then after that I use the Kalman filter to calculate the angle.

I don't have much experience with the MPU6050, but you should have a look at this library for Arduino: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
Actually it looks like you are already using it. Try this example sketch: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino and uncomment the following line to print pitch, roll and yaw: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino#L95

I also use the same approach as you to find the zero value. For instance take a look at my balancing robot source code: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L317-334


The latency in that video looks close to nil so the loop rate must be pretty low. I really don't know what I'm doing to overflow the buffer, or wherever my problem is happening.

Would it matter that I don't have a delay() in my Arduino code at all? I'm only polling for data (on the computer) every 10ms, so perhaps I should throw a delay(5) in there for good measure?


You can always try to add a delay and see if it helps. Why don't you just send the data as fast as possible. I'm sure the computer can handle it? :)


I'm doing that but sometimes it craps out, which is weird as I'm using serial call-response, so I shouldn't be asking for data until it's ready for it.

Sometimes it works fine for over an hour and other times it drops after a couple of minutes.

Stone cold bummer.



I'm sorry .. I use the same sensor.

How did you find these values?

double zeroValue[5] = {950, -400, 13500, -100, -500};

you can edit your skecth?


Sep 14, 2012, 08:40 pm Last Edit: Sep 14, 2012, 08:43 pm by freak174 Reason: 1


I'm sorry .. I use the same sensor.

How did you find these values?

double zeroValue[5] = {950, -400, 13500, -100, -500};

you can edit your skecth?

Hello! I dont have the sketch no more, I partly gave up on this sensos hehe.
However, if you look at Lazarus sketch it is basically the same https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L317-334

But Id recommend you to use this sketch instead as it is made for your sensor https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino#L95

When I used the later mentioned sketch I got it to work with the yaw,pitch,roll values but when I try to combine it to work with a servo I get fifo overflow and the code gets screwd up.



This is awesome, and probably just what i need!

I'm planning to create a gyroscopic platform on which i will mount a GoPro camera or a Contour camera. I will be mounting this onto a motorbike on an actual race track, so it will go through some pretty intense forces. Then i want to compensate the camera angles using e few servos. I only need to compensate for the pitch and roll at this time.

I have experience with arduino's and the coding but im not familiar with the gyro and accelerator sensors.

I do have a few questions:

Offcourse the first question is; considering the forces applied on the sensors by putting it on a motorbike. Will this still work?

and; is using a gyro and accelero enough to fully remove the drift or will i also need the magnetometer (i need the platform to remain absolutely level to the human eye)?

and; are the resulting values usable to compensate using servo's?

i will probably be using this board with my arduino:


Thanks in advance,



It should work perfectly fine if you can find some servos that are strong enough to handle force.
You don't need to use a magnetometer an accelerometer and gyro should be fine.

I actually got some example code for that IMU: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/ITG3205_ADXL345/ITG3205_ADXL345.ino

Also take a look at this project a guy posted at this post a while ago: http://arduino.cc/forum/index.php/topic,58048.msg508589.html#msg508589
And his post as well: http://arduino.cc/forum/index.php/topic,68755.0.html


Go Up