# Kalman filtering demo

By , 2 Aug 2012

## Introduction

Kalman filters allow you to filter out noise and combine different measurements to compute an answer. They are a particularly powerful type of filter, and mathematically elegant. Without a matrix math package, they are typically hard to compute, examples of simple filters and a general case with a simple matrix package is included in the source code. Developed by Rudolf Kalman and others as an ideal way to estimate something by measuring something, its vague applicability (estimate something by measuring something) is where it is so powerful, there doesn’t need to be a direct relationship between measured and estimate.

## Background

### Example 1

Suppose you need to measure a car’s position, but your position estimate is accurate to +/- 10 meters at any one time, but the velocity measurement is accurate to 0.1 meters. Further if the velocity noise error is due to a different physical process than the position measurement, both the velocity and position measurements can be combined in a Kalman filter to compute the position. It will be particularly accurate because the error biasing any one estimate due to position errors will be mitigated by velocity and vice versa.

### Example 2

Suppose you have GPS ranges to satellites (called pseudo ranges) the errors of any one set of measurements will be uncorrelated: the nominal case is when one is higher the other will be lower. They have separate clock based measurement errors and pass through different areas of the atmosphere. The solution of computing position from the pseudo ranges can be done by matrix least squares mathematics, which can be rearranged as an iterative non-linear solution that … improves an estimate of the location till the error approaches zero. Good GPS systems usually take that formulation a bit farther and rewrite it as a Kalman filter. A Kalman filter for navigation can also combine the Doppler (different kind of noise) accumulated carrier, fractional carrier, accelerometers etc.

## Demystifying Kalman Filters

There are typically 2 sets of equations: one to update the “state”, another to use the latest “state” computed in the past to predict what the system is or will be in the future. For example if you get measurements 10x a second but need to make rocket course corrections 100x a second, you update the filter at 10x, but in between updates you predict the state forward to make the course corrections 10 time.

The general theory is to add the measurement into the current estimate using a gain based on how certain the last estimate was. If the estimates are stable, the gain is small and only a small effect of the measurement is filtered in.

Predict:

X = F*X + H*U Rolls state (X) forward to new time.

P = F*P*F^T + Q Rolls the uncertainty forward in time.

Update:

Y = M – H*X Called the innovation, = measurement – state.

S = H*P*H^T + R S= Residual covariance transformed by H+R

K = P * H^T *S^-1 K = Kalman gain = variance / residual covariance.

X = X + K*Y Update with gain the new measurement

P = (I – K * H) * P Update covariance to this time.

Where:

X = State

F = rolls X forward, typically be some time delta.

U = adds in values per unit time dt.

P = Covariance – how each thing varies compared to each other.

Y = Residual (delta of measured and last state).

M = Measurement

S = Residual of covariance.

R = Minimal innovative covariance, keeps filter from locking in to a solution.

K = Kalman gain

Q = minimal update covariance of P, keeps P from getting too small.

H = Rolls actual to predicted.

I = identity matrix.

The key to a good Kalman filter design in my experience is noting how each measurement can be transformed into the state, and how the noises of state and measurements match up in terms of how they are related or not. In the above equations, each letter is not a single number but an object (matrix).

Keeping the order (matrix size) small or at least sparse (lots of zeros in the matrix) is important. The key thing in the equations above is S^-1 is the inverse of S (equivalent of multiplying by 1/S). The inverse of S isn’t always something you can just compute, so usually a pseudo inverse is used. This can be hard to compute if there are values near zero (round off). In short … either do a lot of reading and work and learn about the different approaches or keep the matrix small and or use a math package.

It is usually best from an efficiency and ease of use standpoint to break each thing into a small matrix based process and chain them together. This is usually a good idea because each process in the chain will likely be easier to understand, describe and test. Each will also have it's own special cases for handling reset, coast due to lack of data, bounds checking due to boundary conditions, etc.

## Using the code

Suppose you get a position measurement when you can (time between measurements vary) and you know the position is noisy, but the average velocity of what your measuring should be changing slowly and very smoothly. This is a second order, time varying Kalman filter.

X = a vector, X[0] =position, X[1] = velocity.

P = a 2x2 matrix (4 numbers)

Q = minimal covariance (2x2).

R = single value.

H = [1, 0], we measure only position so there is no update of state.

To estimate forward from this time by delta (dt):

X = F*X + H*U

P = F*X*F^T +Q (computed below during update).

F = 2x2 matrix: [1, dt], [0, 1].

U = 0, If we knew position always increased at baseline velocity, U would be a function of dt.

So H*U = 0

X[0] = F[0, 0]*X[0] + F[0,1] * X[1] = 1 * X[0] + dt*X[1] = X[0] + dt * X[1]

X[1] = F[1,0]*X[0] + F[1,1]*X[1] = 0 * X[0] + 1 * X[1] = X[1].

```double EstimatePredicition(double dt)
{
return m_x[0] + (dt * m_x[1]);
}```

To update:

Y = M – H*X Called the innovation = measurement – state transformed by H.

S = H*P*H^T + R S= Residual covariance = covariane transformed by H + R

K = P * H^T *S^-1 K = Kalman gain = variance / residual covariance.

X = X + K*Y Update with gain the new measurement

P = (I – K * H) * P Update covariance to this time.

Predict:

X = F*X + H*U Rolls state (X) forward to new time.

P = F*P*F^T + Q Rolls the uncertainty forward in time.

The X = F*X + H*U was converted into code above:

oldX = m_x[0];

m_x[0] += dt * m_x[1]; // From above.

Converting P requires some math:

P = [1, dt] [p00, p01] [1, 0] + [q00, q01]

[0, 1] [p10, p11] [dt, 1] [q10, q11]

P = [p00 + dt*p10, p01 + dt*p11] [1, 0] + [q00, q01]

[p10, p11] [dt, 1] [q10, q11]

P = [p00 + dt*p10 + dt*p01 + dt*dt*p11, p01 + dt*p11] + [q00, q01]

[p10 + dt*p11, p11] [q10, q11]

In code:

m_p[0] = m_p[0] + dt*(m_p[2] + m_p[1]) + dt*dt *m_p[3] + m_q[0];

m_p[1] = m_p[1] + dt*m_p[3] + m_q[1];

m_p[2] = m_p[2] + dt*m_p[3] + m_q[2];

m_p[3] = m_p[3] + m_q[3];

Y = M – H*X

y0 = m - m_x[0];

y1 = ((oldX-m)/dt) -m_x[1];

S = H*P*H^T + R

Because H = [1, 0] this is easy, and s is a single value not a matrix to invert.

s = m_p[0] + m_r;

K = P * H^T *S^-1

K = m_p[0] / s;

X = X + K*Y

m_x[0] += y0 * k;

m_x[1] += y1 * k;

P = (I – K * H) * P

Because H is [1, 0], and K is s scalar in this case.

`for (int i = 0; i < 4; i++) m_p[i] = 1 – k*m_p[i];`

The following fills out the API:

```double Variance () { return m_p[0]; } // positional variance.
double Value() { return m_x[0]; } // latest position.
void Reset(
double qx, // Measurement to position state minimal variance.
double qv, // Measurement to velocity state minimal variance.
double r, // Measurement covariance (sets minimal gain).
double pd, // Initial variance.
double ix) // Initial position
{
m_q[0] = qx; m_q[1] = qv;
m_r = r;
m_p[0] = m_p[3] = pd;
m_p[1] = m_p[2] = 0;
m_x[0] = ix;
m_x[1] = 0;
}```

## Another example

Suppose you had two measurement of the same thing, say position measured by GPS, and velocity measured by an accelerometer. The measurement techniques do not vary the same way because the sources of noise are unrelated (little noise cross correlations) and the amount of noise is typical of a measurement system, it is Gaussian. The position noise is large, say 15 meters, but the velocity noise is low, say 0.01 m/s.

This is a good example of how a Kalman filter can really use the low noise velocity information to fix position information that might be noisy.

The Kalman filter learns the velocity over time, and essentially trusts the velocity information to update the position estimate more than the position measurement.

If we introduced a jump in position, the position would jump and the filter would re-adapt.

If the change evolved with the signal like most tracking signal problems do, then you have scenarios such as:

In this case, the velocity went from 0.4 to -0.4 in the middle of the test. The abrupt change in velocity was smoothed based on the noise value used. If we reduced the Qv value to say 0.001 from 0.01 in the above graph the filter would rely on velocity too much:

Example of tracking a parabola with a 2nd order Kalman filter:

Note the top right is the path, top left is the error showing measurement error and the Kalman filter error from the ideal. The Gain plot is the magnitude of the determinant of the Kalman gain matrix (which isn’t a single value).

## Conclusion

This is meant to just be an introduction, a jumping off spot for someone kind of new to Kalman filtering.

On the job, I've developed Kalman filters for image object tracking, missile guidance filters, uplink telemetry timing filters, GPS tracking and navigation filters. Some were small (2-4 state) some large (13) some very large (23). Some took days, others weeks. The most effective and reliable were always small.

Usually it isn't easy. Especially if you have three or more states. By far the most common mistake is making them too big and never quite finding out what's wrong. Figuring out the cross interactions is a science, but it's often good to be deliberately a little more or less aggressive in how it smooths or how quickly it adapts, so... doing that for 20 seemingly unrelated things is tuning 20*20 often non-linear things, by hand.

Kalman filters always work best when they are kept as basic as possible.

 HoshiKata Software Developer (Senior) KMC Systems United States Member
Phil is a Principal Software developer focusing on weird yet practical algorithms that run the gamut of embedded and desktop (PID loops, Kalman filters, FFTs, client-server SOAP bindings, ASIC design, communication protocols, game engines, robotics).

In his personal life he is a part time mad scientist, full time dad, and studies small circle jujitsu, plays guitar and piano.

Votes of 3 or less require a comment

 Search this forum Profile popups    Spacing RelaxedCompactTight   Noise Very HighHighMediumLowVery Low   Layout Open AllThread ViewNo JavascriptPreview   Per page 102550
 View All Threads First Prev Next
 gps smoothing Bob Zoo 27 Sep '12 - 23:50
 Thamks for the great article.   How would you go about calling the code in order to "smooth" gps input ?   I have an application that collects gps data but these are too noisy to use (off a phone) .   Could you please give a small hint towards this ?   Thanks Sign In·Permalink
 Re: gps smoothing HoshiKata 28 Sep '12 - 6:41
 Your not the only one who's asked that. When you smooth GPS input, you can either "filter" the raw data (distances and velocities) then calculate your own position, OR you can smooth the resulting position. On a phone, I'm thinking your smoothing your actual position. Simplest: Three separate filters, each 2nd order (position, velocity) one for X, one for Y, one for Z (or lat, long, alt). You could record your data, pull it into the provided app, calculate decent gains and be done. Kalman will general improve worst case a good bit and cut the error 1/2 to 1/4 depending on luck, tuning, GPS. More complex: Fold in phone's accelerometer data. Gang the filters: Accelerometers + GPS velocity as 2nd order (velocity - accel) filters. Then take the output of the accelerometer/velocity as filtered velocity. That data will be generally very smooth / accurate. Take the velocity output and put it into the filter described in the simplest case. With this and a decent accelerometer and GPS you could get stable sub-meter results. Very complex: Use accumulated carrier and fractional carrier to solve for a stable carrier phase measurement to each satellite by using pseudo range (2nd order). Kalman filter each satellite's range using carrier, carrier rate, range, doppler (2nd order dynamics but 4x4 matrix). Use non-linear least squares to calculate position. Using methods like that (which are text book documented but hard to find) I got 2.2 mm stable accuracy Took about a year of work.   Someday I'll post part 2 of this article, codes done, just no time to write the article. Working on it.   -phil Sign In·Permalink
 Re: gps smoothing Bob Zoo 28 Sep '12 - 21:45
 Phil thanks for the quick reply. I would probably opt for the option (1) as very high accuracy is not really needed - just a ballpark correction.   I have data streaming in via the GPS and for the time are entered in an array of points : route[].long route[].lat route[].alt route[].speed   Could you (if possible) post a snippet that would tie this to the code you posted?   Sorry to be a pain but i am not really sure i "grasp" the kalman filter yet !!!   thanks bob Sign In·Permalink
 Re: gps smoothing HoshiKata 30 Sep '12 - 9:11
 I just submitted my second article, that shows merging accelerometers w/ GPS.   This is what I'd do with either code: Kalman1D [] kf = new Kalman1D[4];   for (int i = 0; i < 4; i ++) { kf[i] = new Kalman1D(); kf[i].Reset(1,1,0.1,0.1); // That would be ok for meters for GPS, // not sure what your units are for // lat, long, alt. }   // Update as a new route point was added. // Return the best estimate smoothed route Route Update(Route route, double dt) { Route r = new Route(); r.long = kf[0].Update(route.long, dt); r.lat = kf[1].Update(route.lat, dt); r.alt = kf[2].Update(route.alt, dt); r.speed = kf[3].Update(route.speed, dt); return r; }   That would be the simplest. The gains 1,1,0.1,0.1 you'd need to set based on the uncertainty of the GPS. Example, if your altitude had an uncertainty of say 2 meters STDEV, then the altitude kalman would be roughly 2, 2, 0.2, 0.2 for a first pass guess at a reasonable value. If your longtitude was in degrees, your uncertainty of a single measurement might be 0.001, so: 0.001, 0.001, 0.0001, 0.0001. Then you could graph each independently and tune the values for each independently. I used a loop to "reset" them just because I don't know what units you have.   Hope this helps.   -phil Sign In·Permalink
 Re: gps smoothing Bob Zoo 30 Sep '12 - 22:33
 Hi Phil,   Great work thanks,   Quick questions: (mostly yes/no answers so I am not to much of a bore!!)   All my measurements are doubles, the structure of the route[] array is all doubles. (long,lat etc)   1. kf[i].Reset(1,1,0.1,0.1); // That would be ok for meters for GPS, Is there a param missing here, the last param should be " ,0 " ? I assume so.   2. From what i understand you make a "new Kalman1D()" for each of the readings (lon,lat etc) is what i understand correct ?   3. You call route update on each position received and in the function you call kf.update for each of the individual kf (lon,lat,etc) . You then combine all these three to make a new position (NEW lon,lat etc) . The resulting new position should then be entered in the "big" array of positions ? Again is this correct understanding ?   IF I have understood point (3) correctly does the update() function do an evaluation and update the matrices ?? Also in the parameter "dt" which is as per the code a double should i enter the seconds elapsed from the past reading ?   best regards and THANK you for all yr kind help.   Bob. Sign In·Permalink
 Re: gps smoothing HoshiKata 1 Oct '12 - 15:38
 1. Yes I think 2. Yes. If your not sure how they are inter-related (merging in velocity) it would be a pain to do otherwise. 3. Maybe. If route[] is the filtered values then yes. If route[] is a record of raw input then no.   The update function does the evaluate and updates the matrices. Yes, seconds from last reading.   You'll need to do a few practical things like reset the filter if you loose incoming data for a long time, etc.   -phil Sign In·Permalink
 Re: gps smoothing Bob Zoo 1 Oct '12 - 0:32
 Re: gps smoothing HoshiKata 1 Oct '12 - 15:39
 Phil Hi,   I have the following but the correction is minimal.   the array data[] is made of long,lat positions already read by the gps and trying to smooth them via the loop.   Also I notice the any value going in to Dt does not change the outcome.   I am sure i am missing something here ? Any ideas ?   Thanks Bob     ```kf[0] = new Kalman1D(); kf[1] = new Kalman1D(); for (i = 0; i < data.Count; i++) { if (i == 0) { kf[0].Reset(0.001, 0.0001, 0.0001, 0.00001,data[0].Longitude); // i use the initial position to be the data[0] eleement - in you example you use 0 . But zero peoduced very high corrections of initial points (placing the position in very far off. kf[1].Reset(0.001, 0.0001, 0.0001, 0.00001,data[0].Latitude); } double kalmanLong = 0; double kalmanLat = 0; double dt = 1; if (i < data.Count - 1) { dt = data[i + 1].Time.Subtract(data[i].Time).TotalSeconds; } kalmanLong = kf[0].Update(data[i].Longitude, dt); kalmanLat = kf[1].Update(data[i].Latitude, dt); data[i].Longitude = kalmanLong; data[i].Latitude = kalmanLat; }``` Sign In·Permalink