Taking a stab at Kalman filtering for IMU

ry.david

Hey everyone, I wanted to use Kalman filtering in my IMU project, consisting of the Sparkfun 2DOF.· The code I based off is from http://www.innovatia.com/software/papers/kalman.htm.· I also used http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data·as a reference.· I understand the concept pretty well, but having problems with the math. Here is my first stab at some psuedo code:

% Copyright 1998 Innovatia Software.  All rights reserved.
% [url=http://www.innovatia.com/software]http://www.innovatia.com/software[/url]
 
measnoise = 10; % position measurement noise (feet)    <- Don't know how to find this value for equipment
accelnoise = 0.5; % acceleration noise (feet/sec^2)    <- Ditto
 
a = [noparse][[/noparse]1 dt; 0 1]; % transition matrix
c = [noparse][[/noparse]1 0]; % measurement matrix
x = [noparse][[/noparse]0; 0]; % initial state vector

xhat = x; % initial state estimate
 
Q = accelnoise^2 * [noparse][[/noparse]dt^4/4 dt^3/2; dt^3/2 dt^2]; % process noise covariance
P = Q; % initial estimation covariance
R = measnoise^2; % measurement error covariance
 
% set up the size of the innovations vector
Inn = zeros(size(R));
 
Repeat
    u = LASTGYROMEASUREMENT
    x = (a * x) + (b * u);     <- Is this right?  What is 'b'??
    z = c * x + MeasNoise;
 
    % Innovation
    Inn = ANGLEFROMACCEL - (c * xhat);
 
    % Covariance of Innovation
    s = c * P * c' + R;
 
    % Gain matrix
    K = a * P * c' * inv(s);
 
    % State estimate
    KALMANOUTPUT = a * xhat + K * Inn;
 
    % Covariance of prediction error
    P = a * P * a' + Q - a * P * c' * inv(s) * c * P * a';
 

Can someone help me with this and make sure I am on the right track.· The matrices are messing with me... Is there a easier way?· Also, I have no idea what the variable 'b' is susposed to be/defined as.

Thanks everyone,
Ryan

deSilva

Ryan,

"b" is just [noparse][[/noparse]dt;0] in terms of your "pseudo code", as "a" is [noparse][[/noparse][noparse][[/noparse]1 -dt; 0 1]; you most like slipped a "minus" here. This is so to make the matrix voodo go!

Whithout any idea about the "noise" the filter will be nearly useless: The algorithm must "know" how "good" (or bad) the measurments are, so it can assess their validity so to speak.

I should urgently recommend you make your program run on a PC before you port it to the Propeller. You have a lot more debugging support there which you most likely will need....

Just log some date of your system and replay it on the PC! Whatever you mean by "...the matrices are mesing with me..." you have to thoroughly understand how they work, as you have to code it line by line - no MATLAB on the Prop

Drone

Hi Ryan,

You might want to have a look at the online interactive Kalman Learning Tool in the links below; in-particular the accompanying explanation and MATLAB files (links provided below). Perhaps you can duplicate the tool's system in your own code then check your results interactively against the tool online. The tool allows you to step through results as well as graph. Thanks to Greg Welch and Gary Bishop at the University of North Carolina at Chapel Hill for these pages.

1. The Kalman Filter:

www.cs.unc.edu/~welch/kalman/

2. An Introduction to the Kalman Filter:

www.cs.unc.edu/~tracker/ref/s2001/kalman/index.html

3. Kalman Filter Learning Tool (Req. Java):

www.cs.unc.edu/~welch/kalman/kftool/

4. Explanation that accompanys the Kalman Filter Learning Tool:

www.cs.unc.edu/~welch/kalman/media/pdf/kftool_models.pdf

5. Downloadable MATLAB files that accompany the Kalman Filter Learning Tool:

www.cs.unc.edu/~welch/kalman/media/misc/kftool_matlab.zip

Good Luck,

David

Drone

Hello Again Ryan,

1. According to this site:

www.geology.smu.edu/~dpa-www/robo/nbot/

"Nuts&volts Magazine for November and December 2006 have a two-part Personal Robotics article by Phil Davis and Brandon Heller, titled "Building a Balancing Bot on a Budget." Their design is based around a SparkFun 5DOF IMU and modified R/C servos, and the article includes a link to their Kalman filter and balancing code on the Nuts&Volts ftp server."

The December 06 Nuts&Volts page is here:

www.nutsvolts.com/toc_Pages/dec06toc.htm

The heavily commented source code may be downloaded here. The source is in C, I don't know the target platform or development environment as I don't have a copy of the Nuts&Volts article:

www.nutsvolts.com/toc_Pages/TOC_Related_Info/0612/PersonalRobotics.php

2. This open project is using a Kalman filter, code should be available from the site:

autopilot.sourceforge.net/

3. You stated, "The matrices are messing with me...". What is your problem with the matrices? If it is understanding the mathematics of the Kalman filter, I presently can't help you. If it is manipulating the matrices in software, then do a Google search for "Numerical Methods" (keep the " "s). You will find lots of help on the Web with how to handle matrix math in software. I'm thinking about implementing a Kalman filter (or something similar) in Propeller to discipline an ovenized crystal oscillator. Though I haven't started on the filter portion of the project yet.

Regards,

David

Post Edited (Drone) : 7/8/2007 12:58:34 PM GMT

Bryan K.

I have tried copying their parts and design in that article, and have had trouble finding all of the necessary components. Of the things I did get, The sparkfun IMU was good for the price, but tended to drift A LOT, and give some erroneous results to the ADC.

JWood

Ryan,
I used the 5DOF sensor from spartfun with good success. after a little strugle with a kalman filter I got it working. It did take a some help from this forum. Any way you can get the spin source from this post: http://forums.parallax.com/showthread.php?p=696561.

I hope this helps.