(1) Why am I bothering to write this?

Every now and again, somebody poses a problem on the forums with a reply like "Just use a Kalman Filter!"

Problem is, few people understand what a KF is, let alone how to use one, let alone how to use one effectively! This tutorial aims to be a gentle introduction as to how a KF works, without going into the maths behind it. It's equation free!

If you want to know the nitty-gritty details, wikipedia [1] has an excellent overview of the equations. If you're desperate for more theory, see Brown & Hwang [2] or various references on the web like this one [3].

If you "just want code", you're missing the point! The code bit is just some matrix equations. The hard bit is modeling it correctly.

---------------------------------------------------

(2) So what is a Kalman Filter?

First and foremost, it's not a filter! Certainly not in the sense that it is explicitly designed to block certain frequencies and pass others through. Instead, a KF is actually an estimator [4] that is designed to make a best-guess at a signal in the presence of noise from a statistical sense, rather than for a specific frequency response. It's much more related to State Space Control and Least Squares Regression than Fourier techniques.

There's two parts to the KF:

(a) An estimation of your dynamics (for this example, a vehicle's position and velocity); and

(b) A noisy measurement of your position/velocity.

A KF works out the Best Possible* compromise between guessing your motion and measurements from a noisy sensor. Yes, even GPS is noisy (about 5m is position, 5cm/s in velocity for good GPS conditions) although much less than other position sensors.

In your KF you'll have a model of your motion. Let's say, traveling at a constant velocity, which is a reasonable assumption for a car on a highway. However, it's not perfect - there will be small accelerations, movement left and right, etc - this is "noise" on your motion estimate.

-------------------------

(3) When do I use a Kalman Filter?

In short, if you don't think you need one, you probably don't! One example that comes up every so often is using a KF for attitude determination from gyros and accelerometers where a complementary filter does the trick quite nicely.

A KF comes into it's own in the following scenarios:

(a) Where the motion of the platform is reasonably well known, measured with a noisy sensor.

(b) Where there are other noise types on a sensor other than white Gaussian noise**

(c) Where there are several different sensors measuring (directly or indirectly) the same thing ***

(d) Where measurements can drop in and out ****

(e) Where you need to know how certain you are of your state (e.g. error on your position/velocity).

If one of those are true, maybe you should consider a KF.

-----------------------------------

(4) So how do you actually do the filtering?

You need to track a few things for a KF:

(1) Your **estimate** of your current position and velocity - this is your state vector

(2) Your **uncertainty** of your estimate of your position and velocity - your state covariance.

Now you know this, here's the process:

(1) From your current estimate of your position and velocity, guess where you're going to be at your next step from your motion model. However, because you don't precisely know your motion, your uncertainty will also grow. We're predicting your next state and next state covariance.

(2) Take a measurement (sometimes called an observation) of your position and velocity from your sensor at the time that you're predicting for. Because there is noise on the sensor, it is an estimate of your next state, and the noise forms your measurement covariance which is a description of how accurate you think your sensors are.

(3) Work out the difference between your measurement and what you predicted your next position/velocity was. This is your measurement residual (or innovation).

(4) Here's the tricky part. Work out the uncertainty on that difference between your measurement and the predicted position - your innovation covariance. We need this to work our which one we trust more - the measurement or the motion model.

(5) Calculate the Kalman Gain, which is the factor on how much we weight the motion model vs the measurements - it comes from the residual and the innovation covariance we calculated before. This is the real "magic black box" of the whole process.

(6) Correct the prediction of your next state by how much of the measurement you trust. That is, take that magic Kalman Gain, multiply that by your residual and add that to your predicted next state.

(7) Adjust your state covariance. By taking a measurement, your uncertainty in your position and velocity will shrink (usually!).

Repeat steps 1-7 forever.

The hard part of a KF is not the equations - it's just some matrix multiplications - it's in the modelling of your sensors and your platform dynamics. It can only be described by linear equations - one without sin(), cos(), x^2 or pretty much anything else. It's very rare indeed that a system is purely linear - most of the time you've got to use some tricks to make it linear, or do something funky like an Extended Kalman Filter.

------------------------

(5) I want to know more!

If you have a copy of Matlab or Octave (Octave is completely free), there are plenty of tutorials where you can see this in action - Matlab Central has about half a dozen implementations.

---------------------------

(6) Can it be done on an AVR?

Short answer: Yes

Longer Answer: It's been done on (for example) the MicroNav project [5]. In general, it can handle low-sample rate implementations where there aren't too many states to estimate and aren't too many measurements to process. Matrix Inverses are pretty nasty operations!

--------------------------------

(7) Can I copy this tutorial?

You may distribute this article under the terms of the Creative Commons Attribution-NonCommercial-ShareAlike License - see http://creativecommons.org/licen...

Bug-fixes, corrections, comments welcome.

-- Damien

---------------------------------

Footnotes:

* Best Possible: For a KF, it can be in the "least square error", "expectation maximisation", "Best Linear Unbiased Estimator (BLUE)" or "minimum variance unbiased (MVU)". Take your pick - they're all Googleable.

** Other types of noises exists on many sensors. For example, gyros and accelerometers have a slowly and randomly changing bias with time that can be modelled in a KF, if there is some other sensor that makes them observable (e.g. a GPS/INS implementation)

*** For example, a wheel odometer can be used at the same time as GPS, and can be used where GPS is unavailable (e.g. blocked by buildings)

**** GPS being blocked by buildings is a good example. Or, if doing a raw GPS implementation, each satellite is a different measurement. The number of satellites seen changes constantly.

---------------------------------------------------

References:

[1] http://en.wikipedia.org/wiki/Kal...

[2] http://www.amazon.com/Introducti...

[3] http://www.google.com.au/url?sa=...

[4] http://en.wikipedia.org/wiki/Est...

[5] http://sourceforge.net/projects/...

--------------------------

Edit History:

1.0 - Initial Issue

1.1 - Typos and minor cleanups

1.2 - Fixed heading numbers as per comment. Thanks!