Kalman filter – simplified version


Finally… it’s been a while since I’ve started looking into filtering, and more precisely Kalman filters… since I started on my quadcopter project, to be more precise…

After reading the descriptions and formulas found here (highly recommend the short introductory pdf !) I decided I had to code my own simplified version.

Kalman filter applied to input from a noisy FM receiver

The data I’m using for this post comes from a (very) noisy RC FM receiver that sits right next to an electric motor and that was causing troubles to the point that I couldn’t control the tank (even with a capacitor on the motor !).

So let’s start with simply filtering a scalar value…

Here’s the Java code :


package mykalman;

public class MyKalman {
private double Q = 0.00001;
private double R = 0.01;
private double P = 1, X = 0, K;

private void measurementUpdate(){
K = (P + Q) / (P + Q + R);
P = R * (P + Q) / (R + P + Q);
}

public double update(double measurement){
measurementUpdate();
double result = X + (measurement - X) * K;
X = result;

return result;
}

private static int[] DATA = new int[]{0,0,0,0,0,0,0,0,0,0,3,13,20,31,39,44,46,54,57,57,63,67,67,70,76,78,86,0,93,99,100,100,100,100,102,106,108,105,50,108,106,106,106,105,105,105,106,106,106,105,105,108,106,80,106,106,105,106,106,106,106,106,106,106,2,106,106,106,106,108,106,108,104,106,105,106,105,106,106,105,105,105,106,106,106,106,105,106,106,105,105,108,30,106,106,105,106,106,104,106,106,105,80,109,106,108,105,106,106,106,108,106,106,105,106,104,108,105,106,106,104,106,106,106,108,108,106,45,60,75,106,106,106,106,105,105,104,106,106,106,106,108,108,106,106,106,105,105,106,106,106,106,106,106,106,105,105,109,106,108,106,106,105,106,108,20,106,106,106,105,105,106,104,108,108,106,106,104,106,104,106,105,106,106,109,45,106,106,106,66,105,106,103,105,105,106,106,105,108,104,106,105,106,106,108,104,105,66,66,66,106,105,108,105,108,106,106,106,106,106,106,108,106,108,105,105,105,106,106,105,106,108,106,105,106,0,106,105,106,106,106,106,106,104,106,104,108,106,108,108,106};

public static void main(String[] args){
MyKalman filter = new MyKalman();
for(int d : DATA){
System.out.print(Math.round(filter.update(d)) + ",");
}
}

}

Yep, that’s it…. no kidding… it’s obviously quite simple because :

  • there is no control input
  • we measure the state directly
  • etc. …

But still does a quite impressive job !

Advertisements

12 Responses to Kalman filter – simplified version

  1. Pingback: 【確率ロボティクス】カルマンフィルタの実装 | GUSUGUSU.NET

  2. Pingback: kalman-filter-simplified-version | 不分享空間

  3. Pingback: a drop in the digital ocean

  4. Two questions:
    1. Is it possible to replace second line in measurementUpdate method with
    P = R * K; ( because (P + Q) / (R + P + Q) is already equal to K) ?

    2. It seems K tends to 0.0311267292017… (after 100-th step) so do we need all that difficult calculations?

    • trandi says:

      You are correct, after a while I did realise that this version of the filter was so “simplified” that it’s basically almost just a low pass filter… but never took the time to update this post.
      Thank you for pointing it out.

      Have a look at some of the comments below, especially the one with explaining that “K and P are actually independent “.

      Dan

  5. Alex says:

    Hi,
    for what does Q, R and P stand for? What I mean: Which of these Parameters I have to change, for example, to get a fast rise?

  6. Do you know of a reliable way to choose a decent process noise and measurement error covariance for use with your filter?

    Thanks!

    • trandi says:

      Hi Bob,

      No, this is barely my first foray into filtering / machine learning, and to be honest I think the example in this post is sooo simplified, that it basically has the same effect as a low pass filter, which is much easier to write…

      But please do let me know if you find a nice / simple process to choose the covariance matrix …!

      Dan

      • Thanks, I’ll let you know if I find anything. I was hoping your solution would be a silver bullet but I think you are right, it might be a slightly better low-pass in its current state.

  7. Pingback: Very Simple Kalman in C# « a drop in the digital ocean

    • trandi says:

      thanks for you reference to my Java Kalman filter implementation.

      Just beware that, because there is no control input K and P are actually independent of X (the process value) and will quickly tend to constant values… hence even that measurementUpdate() method becomes un-necessary and the whole filter will amount to doing a simple moving average over the measured values…

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

%d bloggers like this: