Combine Gyroscope and Accelerometer Data

Question

I am building a balancing robot using the Lego Mindstorm's NXT system. I am using two sensors from HiTechnic, the first being an Accelerometer and the second being a Gyroscope. I've successfully filtered out noise from both sensors and derived angles for both in a range between -90 and 90 degrees, with 0 degrees being perfectly balanced.

My next challenge is to combine both of the sensor values to correct for the Gyroscope's drift over time. Below is an example graph I created from actual data to demonstrate the drift from the gyroscope:

The most commonly used approach I've seen to make combining these sensors rock solid is by using a Kalman filter. However, I'm not an expert in calculus and I really don't understand mathematical symbols, I do understand math in source code though.

I'm using RobotC (which is like any other C derivative) and would really appreciate if someone can give me examples of how to accomplish this in C.

Thank you for your help!

SOLUTION RESULTS:

Alright, kersny solved my problem by introducing me to complementary filters. This is a graph illustrating my results:

Result #1

Result #2

As you can see, the filter corrects for gyroscopic drift and combines both signals into a single smooth signal.

Edit: Since I was fixing the broken images anyways, I thought it would be fun to show the rig I used to generate this data:

Hora

your data is clearly diverging. the kalman filter or any other method won't help you if your initial data doesnt agree. – Wallpaper

I'm not sure you fully understand what the graph is displaying, it's a known problem of Gyroscopic data to drift. They is why the data is diverging, which is what the filter/integration I'm looking for will correct using the accelerometers data. Also, the reason for the radical drift, is because I shook the sensors pretty violiently to illustrate my problem. :) – Hora

I have no idea what you are graphing because you did not label the axis's but regardless if your data is clearly diverging from the same y-values given the same x-values it is pretty bad data. – Wallpaper

if you apply any filter to it as is that tries to minimize error in the least squares sense (what the kalman filter does for example) your going to be averaging an error that increases as your values of x increase. Clearly one part of your data is telling you something and a different part of your data is telling you something else. – Wallpaper

You most likely don't understand the correct model for your data. You probably need to apply a different transform to each group of data you collected before you can compare them apples to apples. – Wallpaper

If you look at the results, the filter clearly accounts for the drift. I know the drift is caused by manufacturing tolerances, and rounding issues. Honestly, you're correct though, because the data I'm displaying is basically a running total of the gyro offset by adding the angular change every iteration, the complementary filter tracks the angle by taking the accelerometer's data into account as well and coming up with a more accurate blend of both values, thus correcting the gyroscopic drift. That's for your help gmatt. – Hora

A great alternative to the Kalman filter is the complementary filter which is much easier to implement: http://www.pieter-jan.com/node/11 – Rest

The images are no longer showing up on this question. – Adiell

I've fixed the broken images. – Hora

I enjoyed this question and answer so I was going to up vote it... However, I noticed that you were drinking Miller Lite... so I have to down vote this based on moral grounds. – Highflier

We all have to stand up for what we believe in, and I respect your right to express yourself how you see fit. :) – Hora

Answer 1

Kalman Filters are great and all, but I find the Complementary Filter much easier to implement with similar results. The best articles that I have found for coding a Complementary Filter are this wiki (along with this article about converting sensors to Engineering units) and a PDF in the zip file on this page (Under Technical Documentation, I believe the file name in the zip is filter.pdf);

PS. If your stuck on a Kalman Filter, here is some C-syntax code for the Arduino that implements it.

Dislocate

FANTASTIC, I believe this may be exactly what I was looking for. The Filter.pdf file was really the big help, and explained and solved my exact problem. I haven't verified it yet (I'm at work). But tonight, I'll try and get this going and mark my question as answered! – Hora

Glad I could help! If you want to see an example of it in action, check out my blog at ohscope.com. I built a Segway like balancing scooter, and I will be putting up more data soon. – Dislocate

The wiki link appears to be dead – Sancha

Yay archive.org: web.archive.org/web/20091121085323/http://www.mikroquad.com/bin/… – Jacalynjacamar

Answer 2

Gamasutra.com ran an article on using Kalman filters for WiiMote filtering.

There are some links to C++ source code at the end of the article.

Sportsman

I appreciate your response. This article is also great, but I really need to know how to use a Kalman (or other) filter to combine two sensor values, instead of just smoothing one. – Hora

From what I understand, you smooth one, then use that to apply a corrective factor to the other. Can't say I FULLY understand how it's supposed to work, though. These <a href="tlb.org/scooter2.html">two</a> <a href="tlb.org/scooter.html">articles</a> may be of help. – Sportsman