An Intuitive Introduction to Kalman Filter

The purpose of this tutorial is to illustrate the usage of Kalman Filter by a simple example.

The problem: Predict the position and velocity of a moving train 2 seconds ahead, having noisy measurements of its positions along the previous 10 seconds (10 samples a second).

Ground truth: The train is initially located at the point x = 0 and moves along the X axis with constant velocity V = 10m/sec, so the motion equation of the train is X = X0 + V*t. Easy to see that the position of the train after 12 seconds will be x = 120m, and this is what we will try to find.

Approach: We measure (sample) the position of the train every dt = 0.1 seconds. But, because of imperfect apparature, weather etc., our measurements are noisy, so the instantaneous velocity, derived from 2 consecutive position measurements (remember, we measure only position) is innacurate. We will use Kalman filter as we need an accurate and smooth estimate for the velocity in order to predict train's position in the future.

We assume that the measurement noise is normally distributed, with mean 0 and standard deviation SIGMA