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 !