Guide to gyro and accelerometer with Arduino including Kalman filtering

Hallo everybody
I recently bought this analog 6DOF (six degrees of freedom) IMU board (IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU - SEN-10010 - SparkFun Electronics) 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. The code for Arduino can be found at github: GitHub - TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter: Software for "Guide to gyro and accelerometer with Arduino including Kalman filtering". 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 acceleration (g'a) 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 a ADC with a 10 bit resolution, for example this should be 4095 (2^12-1=4095) for 12 bit ADC). 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 stationary (this is done in the code - look in the "Setup" section) 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.31023.
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 (delta time) and thereby calculate the angle of the gyro. The final equation will look like this:
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 acceleration (g's) in three dimensions. To translate the analog readings into degrees you simply need to read the axis and to subtract the zero offset like so:
accVal = accAdc-accZero

Where accAdc is the analog reading and accZero is the value when it reads 0g - this is calculated in the start of the code, look in the "Setup" section. The zero value can also be found in the datasheet: http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf. You will see that the zero voltage at 0g is approximately 1.5V, to translate this into quids, you again have to use this equation: zeroVoltage/3.31023.
So in this example I get:
1.5/3.3
1023=465.

You can then calculate the pitch and roll using the following equations:
pitch = atan2(accYval, accZval)+PI
roll = atan2(accXval, accZval)+PI

Atan2 has a output range from -? to ? (see atan2 - Wikipedia), I simply add ?, so the range it converted to 0 to 2?.
To convert it from radians to degrees we simply multiply the result by 57.295779513082320876798154814105 - this is predefined in the Arduino IDE as RAD_TO_DEG.

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. A detailed guide on how it's implemented can be found at my blog: TKJ Electronics » A practical approach to Kalman filter and how to implement it.

If you want to use something a bit more simple, you can use what's 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+gyrodt) + 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 attached my own code for my 6DOF IMU (IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU - SEN-10010 - SparkFun Electronics), 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.

Sources:

http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418
http://www.x-firm.com/?page_id=148

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.
The code can be found at github: Example-Sketch-for-IMU-including-Kalman-filter/Graph at master · TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter · GitHub.
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.
The source code can be found at the following link: GitHub - TKJElectronics/BalancingRobotArduino: This is the Arduino version of the code for my balancing robot/segway
Balancing Robot Presentation - YouTube

Kickstarter
I have just released my balancing robot on Kickstarter: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit.
Please consider backing the project.
Balanduino - Balancing Robot Kit - YouTube

Update
I have also provided code for several other IMUs including a 9DOF which allows you to estimate yaw as well.
All can be found in the Github repository: GitHub - TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter: Software for "Guide to gyro and accelerometer with Arduino including Kalman filtering".

1 Like

Thanks for posting this guide. I will give it a go!

Justin

I have updated the code, so it now measures 3600 instead of 1800. Just add this after you calculate the angle measured by the accelerometers:

if(accZval < 0)//360 degrees
  {
    if(accXangle < 0)
    {
      accXangle = -180-accXangle;
    }else
    {
      accXangle = 180-accXangle;
    }
    if(accYangle < 0)
    {
      accYangle = -180-accYangle;
    }else
    {
      accYangle = 180-accYangle;
    }
  }

Awesome work

I am making a 3 axis rate table to measure how accurate the imu is.
I want to answer the following questions
1)For a given set of sensors, how can i get the best possible performance from my Kalman filter in estimating angles.
2)Now that the "optimal" Kalman filter code is identified, can i achieve better performance by choosing better gyros and accelerometers.

Thank you. Are you using the same IMU as me or a different one?
If so will you please tell me what you come up as the best values for Q_angleX, Q_gyroX, and R_angleX?

I am using the Ardu-Imu

R is the co-variance matrix. It should typically be the square of the standard deviation of the Gyro.
I think it should be there in the datasheet, or you could just keep things stationary and log the data for about 5 mins.
Calculate the mean and std deviation.
R=std_deviation^2

Hello,

This is a great post about IMU's. Lots of good information. I am also working with Gyro's and accelerometers in my Quadrotor project. I needed a simple tool to visualize data and ended up writing one. Although there are ways to plot data using Processing, I wanted a stand alone tool.

Here is link to relevant thread. Hope you guys find the tool useful.

http://arduino.cc/forum/index.php/topic,58911.0.html

I will probably change the tools to accept float data in the next version.

Cheers

Aerosam:
What unit do you measure the standard deviation and mean in? For example is it %/C or what?

  • Lauszus

This is a great for IMU-starters like me. I will give it a try.
Thank you very much.

Fons

This isn't working for me. My angle is still drifting at a degree per second.

Are u using the same IMU as me or a different one?
If not, try to change the value of R_angleX and R_angleY and see if it helps.

  • Lauszus

I'm using a different one, the digital combo board. SparkFun 6 Degrees of Freedom IMU Digital Combo Board - ITG3200/ADXL345 - SEN-10121 - SparkFun Electronics. I had to mod the code to suit the digital version, but i'm confident it works.
I've tried changing the values but they don't seem to have any benificial effects :frowning:
Sometimes the result jump about 20 degrees, quite rarely. Does this ever occur for you?

Okay.
It sounds like you are getting data from the accelerometer. I you sure you are using both sensors? :slight_smile:

  • Lauszus

Haha, yeh definitely getting all the data :stuck_out_tongue: For some reason i get more accurate data from the non filtering. The calculated accelerometer angle doesn't go through 360degrees. Gets about a max of 50. Is this supposed to happen?

Try to use the complementary filter instead and see what happens.

Post your code and I will have a look at it - you must have done something wrong :slight_smile:

  • Lauszus

I modded the code into java, as i'm doing the postprocessing on the PC.
the only thing i think i modded is this...
xAngle += kalmanCalculateX(accXangle, gyroXrate, dtime);
I made the xAngle += so I just print total xAngle directly. Is that wrong?
Apart from that, using nano seconds and slightly different calibration position it is the same.

Yes that it totally wrong. If you do like that, it will plus the angle with the last one. It should be:

Angle = kalmanCalculateX(accXangle, gyroXrate, dtime);

It is only when you calculate the gyros angle that you have to plus the last angle with the new one!

  • Lauszus

ok fixed that. But it only works for about 50 degrees +-. Similar to the other kalman filter i tried :frowning:

Maybe you forgot that gyroXrate should be in degrees per second and not degrees in this line:

Angle = kalmanCalculateX(accXangle, gyroXrate, dtime);
  • Lauszus

Hi Lauszus, I bought the same gyro(Gyro Breakout Board - LY530AL - 300°/s - SEN-09165 - SparkFun Electronics) used by IMU on your project. I just want to read angle from the gyro but having a little confuse about the HP, PD and ST pin. Should I just left them all unconnect or do you have any suggestion?

~Thanks :slight_smile:
~Fino