How to implement a position-only Kalman Filter

Hi all, here’s how to implement the Kalman filter from scratch, in C-ish pseudocode. The intended audience is developers, as opposed to mathematicians, so there will be no complicated formulae - just additions, subtractions, and multiplications. I won’t try to explain the theory behind the Kalman filter and its intended uses, that is explained far better elsewhere (Wikipedia is a great place to start). To oversimplify, it’s a little like a moving average.

This article explains how to implement a position-only Kalman filter, as a stepping stone to implementing a position-and-velocity filter in my next article.

Firstly, you’ll need the ‘external noise variance’. This is usually referred to as Q in Kalman literature:

const double Q_EXTERNAL_NOISE_VARIANCE = 0.1

Next, you’ll need the ‘measurement noise variance’, aka R in Kalman articles:

const double R_MEASUREMENT_NOISE_VARIANCE = 0.1

How do you select good Q and R values? I’m not an expert, but here’s my two tips:

• If you’re filtering a sensor that has a normal / gaussian distribution vs the actual value, use the variance of its inaccuracy as R (variance = average[residual^2], residual = measurement - actual value), and experiment with Q until it works the way you’d like.
• If you’re filtering non-gaussian data (eg stock prices), simply experiment with Q and R until you’re happy with it. I’d write a program to generate a million random Q+R pairs, then choose the pair that predicts the ‘next’ data points best. That is an (whopping) exercise for the reader, sorry!

Next, we need our ‘state transition matrix’, aka F in Kalman articles. Since this is the one-dimensional filter, this is a simple double that, when multiplied by a data point, gives us our expected next data point. Eg it’ll simply be 1.

const double F_STATE_TRANSITION = 1

Next, we need the ‘state error’, aka P in Kalman articles. I find it usually works fine to start filtering with this as 1:

double pStateError = 1

Next, we need our ‘current state’, aka X in Kalman articles. Start filtering with this set to the first measurement:

double xCurrentState = data[0]

Now a prerequisite: transposing a ‘matrix’. In our simple one-dimensional case, this is a no-op, but I’m leaving it here as a stepping stone to the next article:

double transpose(double a) { return a }

Another prerequisite: inverting a ‘matrix’. In our simple case, this is just 1 / a.

double invert(double a) { return 1 / a }

Now for the interesting bit. For each new data point/measurement, we need to first predict what it’ll be, then blend that prediction with the measurement to produce an estimate. This estimate is the filter’s output. A ‘Kalman gain’ (known as K in Kalman articles) is calculated that is used to weight the prediction vs measurement when blending them to form the estimate.

// Make predictions.
double xPredicted = F_STATE_TRANSITION * xCurrentState
double pPredicted = F_STATE_TRANSITION * pStateError * transpose(F_STATE_TRANSITION) + Q_EXTERNAL_NOISE_VARIANCE

// Update it with the measurement.
double zMeasurement = data[current_index]
double kKalmanGain = pPredicted * invert(pPredicted + R_MEASUREMENT_NOISE_VARIANCE)
double xEstimate = xPredicted + kKalmanGain * (zMeasurement - xPredicted)
double pEstimate = pPredicted - kKalmanGain * pPredicted

// Setup for the next iteration.
pStateError = pEstimate
xCurrentState = xEstimate // This is the output!

You may find my next article about position-and-velocity Kalman filters helpful!

Thanks for reading, I hope this helps, God bless, and have a nice week :)

Chris Hulbert

(Comp Sci, Hons - UTS)

Software Developer (Freelancer / Contractor) in Australia.

I have worked at places such as Google, Cochlear, Assembly Payments, News Corp, Fox Sports, NineMSN, FetchTV, Coles, Woolworths, Trust Bank, and Westpac, among others. If you're looking for help developing an iOS app, drop me a line!

Get in touch:
[email protected]
github.com/chrishulbert
linkedin

 Subscribe via RSS