Gyro/Accelerometer Fusion

[jwakeman]

I originally created this thread in the Programming forum.

http://www.chiefdelphi.com/forums/sh...d.php?t=113052

It's not really a programming question so I wanted to ask here to see if I could drum up any replies.

[slibert]

Using the accelerometer to correct for gyro drift is a great idea in principle.

However, it's important to understand that the acceleration measured by an accelerometer is actually due to two separate physical quantities.

The first is the "static acceleration" due to gravity. If this were the only physical quantity the accelerometer measured, then you could easily use this to cancel the drift of the gyro!

However, the accelerometer also measures "dynamic acceleration" due to the motion of the object on which the accelerometer rests (the robot).

And it's really not a simple problem to separate the two.

However, as another poster noted, this is what a MEMS-based Inertial Measurement Unit (IMU) does do. There are a range of filtering techniques that are used to do this, including:

- Complementary Filtering
- Extended Kalman Filtering
- Direction Cosine Matrix Filtering

These actually get rather complex and can be difficult to understand, although the Complementary Filter is the simplest and a good place to start.

And if you have the time/interest/patience to learn these techniques, here are some pointers to info that helps.

http://www.instructables.com/id/PCB-...entary-Filter/

http://www.starlino.com/dcm_tutorial.html

http://gentlenav.googlecode.com/files/DCMDraft2.pdf

During the off-season, our team worked on designing a custom IMU circuit board, it's been a great learning experience. We are currently using a DCM approach, but are looking into also using an Extended Kalman Filter to help improve accuracy.

As to hardware, you can also find reasonably inexpensive off-the-shelf IMU circuit boards at Sparkfun, Pololu and some others. These are often referred to as 6-Degrees of Freedom (6-DOF) sensors, and you can find open source software for them (a number of them are Arduino-based). We went this direction because the gyros/accelerometers in the FRC KOP just didn't provide the kinds of accuracy and response we were looking for - and because we wanted to experiment with using a dedicated microcontroller to manage these sensors. This approach allows us to sample the sensors at high rates and execute the filtering algorithms without worry of competing with our other CRio-based software.

You didn't ask about it, but I wanted to mention in the same breath that there are also 9-DOF sensor boards that add a 3-axis magnetic compass. This can be useful, but the compass readings go haywire when near a motor, so they have limited usefulness. We read the compass heading at the start of the match before the motors are enabled, gives us an absolute heading which sometimes proves useful. Depending upon your situation, you may be able to use this as well, just pay attention to the magnetic interference issue.

Hope that helps, but please feel free to follow-up if you have any questions. The Extended Kalman Filter/DCM math is non-trivial (trigonometry and matrix calculations), so if you're like me, you'll need to set aside some time to really understand the algorithms, and have a bottle of aspirin handy. That said, you might start with the simpler algorithm (the Complementary Filter) and see if that is sufficient for your needs. The higher accuracy / lower latency you need, the more complex things get.....

[apalrd]

I worked with direction cosine matrix a few years ago, it was probably one of the coolest sensor algorithms I've seen. Watching an ECU sitting on an angled piece of wood adjust pitch and roll with only a yaw input and accelerometer changes was really cool to see.

That said, I'm quite certain it's not necessary for FRC. The duration of autonomous basically allows us to forget aboout sensor drift, and resetting the sensors occasionally in between moves reduces the effect of drift even more.

My favorite FRC solution would be a encoder-gyro algorithm that can detect gyro drift, and encoder slip.

[jwakeman]

I found this great white paper on this exactly this subject.

http://www.chiefdelphi.com/media/papers/2010