OpenServo.com

[mpthompson]

This is just partially related to the OpenServo (at least for now), but for the last week I've been hitting the books and attempting to understand as best I can the Kalman filter algorithm. This is related to my day job and it may make its way into a future sensor project I'm contemplating.

In any case, I'm implementing a Kalman filter to fuse gyro and accelerometer data from the SparkFun 5 Degrees of Freedom IMU. I thought others with more knowledge of such algorithms may be interested in the code and perhaps comment on it. A copy can be found at:

http://home.comcast.net/~michael.p.thompson/kalman/kalman_test2.c

If I can get things working well enough and optimized to the point it will run on an AVR MCU, I would like to design a small sensor board that will report pitch and roll attitude from the IMU sensors over the I2C bus.

The code is just processing test data for now, but I believe its working well enough to show promise that I can get it working as intended. However, currently I'm stumped on how to determine the process noise covarience matrix. I would be eternally grateful if someone could help walk me through determining the values for that matrix -- as well as confirm my value for the measurement noise matrix.

A benefit of this is that I at least have a somewhat better understanding of the models of the OpenServo that Zed and Stefan have created. I'll have to make my way through their old posts to see if it I can now make more sense of what they were doing.

-Mike

[phooddaniel]

I have been racking my brain trying to smooth out my A/D signals from the freescale accelerometer using an ATMEGA32. I have implemented my own crude and probably inefficent code that does a pretty good job. The filter that you are researching is probably the way I should go.

First a collect a couple of samples then average them. Second, I create a variable that increments or decrements toward that averaged sample so it doesn't spike.

I'm very eager to learn the success of the Kalman Filter.

My circuit is also a possible culprit as my ground plane for my motor shares the same ground plane for the microcontroller and the accelerometer. Once I change that, I should see a cleaner signal. Atmel suggests decoupling and adding an inductor on the Avcc.

[mpthompson]

An AVR Studio project which interfaces an AVR ATmega168 with the SparkFun IMU 5 Degrees of Freedom (ADXL330 accelerometer and IDG300 gyro) can be found here. The README.TXT file describes how to connect the IMU 5 to the AVR STK500 development system for testing.

Information regarding the SparkFun IMU 5 I'm working with can be found here.

The project combines raw accelerometer and gyro data from the IMU to output pitch and roll in degrees along the X and Y axis at a 10 Hz rate out the spare RS232 port at 57.6K baud. It seems to work fairly well, but I'm looking for feedback from others to improve the code and help tune the measurement and process noise covariance matrices. The kalman module in the code is self-contained and should port easily to other platforms.

If you are having noise issues reading your sensor readings you may want to try to implement a digital low-pass filter rather than just averaging the last n values. A good introductory article can be found here. An implementation of the digital filter can be found in the OpenServo source code in the pulsectl.c file.

I hope you find this information useful.

-Mike

[mpthompson]

An updated project file can be found here:

http://home.comcast.net/~michael.p.thompson/kalman/AVR_IMU_2.zip

This new project contains:

1. A bug fix where I was passing in the measured gyro delta between samples rather than the measured gyro rate into the Kalman filter. This resulted in effectively multiplying the gyro rate by the sample period twice.

2. Changed the math to use radians rather than degrees for easier integration with trig functions. The output to serial port is converted to degrees for display.

3. Modified the initialization code to remove all hard coded constants from the kalman.c file to the init function.

4. Clean up of variable names in main.c.

-Mike

[linuxguy]

[mpthompson]

I got stuck with my Kalman filter implementation and adapted the Extended Kalman Filter from the Autopilot Project into my latest AVR IMU code. The project can be compiled to use either algorithm in the imu.h file. A link to this updated project is below.

http://home.comcast.net/~michael.p.thompson/kalman/AVR_IMU_3.zip

A link to the original source code is here:

http://www.rotomotion.com/downloads/tilt.c

I'm very early into all this stuff, but I have learned that when integrating highly non-linear sensors such as accelerometers the preferred filter algorithm is the Extended Kalman Filter or the Unscented Kalman Filter. This pretty much obsoletes my own code and I'll be starting from scratch again on my own implementation.

I have the book Probabilistic Robotics on order so I can hopefully get a much better grasp of this stuff.

-Mike

[jamma]

Mike:

Have you had a chance to play with this enough to comment on the practical differences between your "pure" Kalman code and the EKF alternative? I've gotten sidetracked on a balancing scooter project, and lo and behold stumbled (Googled?) upon your Kalman stuff here and at the SF forums, as soon as I realized I needed a Kalman filter. I'm thinking of porting what you've done to the propeller chip as a learning exercise (in both KFs and the prop).

[mpthompson]

I'm just now building up the robot shown below to utilize the Extended Kalman filter I adapted from the Autopilot project.



As you can see the hardware is just coming together right now. If I can get enough time to work on the software I should have it finished in about two weeks or so. I'll post the results as soon as I get it working.

Once the basics are complete I hope this will be a pretty good platform for experimenting with and refining the algorithms.

-Mike

[HakBot]

Mike,

I just found this code. I was considering experimenting with a gyro acceleromerter combo but the thought of having do create a kalman filter turned me away. Your code is so easy to understand that I might just go ahead and give it a try now! Thanks a lot!

Phil

[mpthompson]

I'm glad you found it useful. As I posted in another thread, I've got the code running in the following robot:

http://www.youtube.com/watch?v=qTGolngNves

I'm using the measurement noise covariance and process noise covariance parameters directly out of the autopilot code and I was pleasantly surprised they worked without change for my specific accelerometer/gyro hardware (or at least well enough). One day I'll have to actually tune these parameters to see if I can get better precision from the filter.

-Mike

[HakBot]

Mike,

I have some questions about your imu hardware. Im experimenting with harware to measure pan and tilt movements. At first I thought that an accelerometer would be enough but I realized it is useless for pan measurements. Which lead me to a gyro....

Anyway, as I understand, the kalman filter will combine both accelerometer and gyro readings to correct the drift in the gyro. What I don't understand is how the accelerometer will help to correct drift on the pan axis?

Thanks!

[mpthompson]

Right now, the code I'm using is only being used to measure the pitch of my balancing robot -- roll and yaw (pan) are ignored. This utilizes accelerometer values along the Y and Z axis and gyro values around the X axis -- 3 of the 5 outputs from the SparkFun 5DOF IMU I'm using.

As you describe, the Kalman filter uses the accelerometer values to correct for drift in the gyro. The problem for yaw (pan) is that you don't have a strong gravitional vector that you can use to correct for gyro drift as the axis of rotation is parallel to Earth's gravity.

I haven't yet done this myself, but I think for panning you want to use a Kalman filter to fuse the outputs from a magnetic compass such as the Hitachi HM55B Compass with the gyro value. In this way, the gyro provides very fast response to quick changes in yaw and the magnetic compass readings correct for gyro drift over time in place of the accelerometer readings.

Of course, a magetic compass unit has it's own set of problems in that it is sensitive to local magnetic fields and such, but this may not be an issue if you are just trying to correct for gyro drift rather than localization relative to magnetic North.

Hopefully this makes sense.

-Mike

[HakBot]

I found this: http://www.rcgroups.com/forums/attachment.php?attachmentid=1073721

The thread here is about creating a head tracking device, basically the same thing I want. Its hard to tell but it looks like he is only using a 2 axis gyro from gyration. The users didnt seem to report any bad drifiting so it must be working correctly. Do you know how he is getting away with only using a gyro?

[mpthompson]

I really don't know. I suppose he could have a high pass filter on the gyro data that removes the drift component which would trade some accuracy. Since it is a person who is viewing the camera data through a head mounted display, it could be that a person will naturally correct the drift by occassionally turning their head slightly against the direction of drift. Humans are very good at such unconscious corrections.

This paper on complementary filters may give you some other ideas beyond using a Kalman filter.

Mike

[HakBot]

Thanks I will give that a read. I've got my gyro on order so its only a matter of time before I can get to work.

Anyway...there is a button that is attached to that particular headtracker which allows the user to re-center the gyro. From reading other posts I got the impression that the gyro would drift off course in a matter of seconds, is this not true? I wont have a human to correct my implementation so im wondering just how bad it can drift over time.