Kalman Filter in a Nutshell

10 min readDec 6, 2020

(Image by Author)

When I first started learning to use Kalman filter, it seemed very complicated to me, mainly because there were so many inputs to it and you never really knew if you’ve set them correctly. By going through this example, I hope my readers will understand that the motivations and logic behind it is really very pragmatic in nature and not so complicated after all.

Drop a Tennis Ball

So let’s get started with our example.

I am dropping a tennis ball from a helicopter (or maybe my airship), I would like to predict its position in the air until it hits the ground.

Tennis Ball Dropping (Image by Author)

You would think this is a very simple problem, right? After all, we’ve got our equations of motion:

Equations of Motion

Where g is acceleration due to gravity (9.81 meters per second squared).

Well, unfortunately the real world is not that simple. If you are dropping a lead ball I suppose the equations of motion are all you need, but for something light like a tennis ball, it’s trajectory might be affected by all sorts of things.

For example, there might be a really strong wind blowing.

Wind Blowing (Image by Author)

Or a really hungry alligator might be near by…

Hungry Alligator (Image by Author)

The real world is complicated.

So, if your prediction solely comes from the equations, it might be really off by the time the ball reaches the ground.

Let’s take a closer look.

Simulation

Let’s simulate what happens.

Suppose we drop a ball 20,000 meters above ground, it will mostly follow the the equations of motion, with random forces such as wind and alligators affecting it’s trajectory:

Where F is the equations of motion we just discussed. G and N are the random forces affecting the ball in real life (wind and alligators). N is basically a random variable of normal distribution, with mean 0 and standard deviation sigma(a).

Our initial conditions, therefore, are as follows:

# initial values
x0 = np.matrix([[20000],
                [0],
                [-9.81]]) # gravity

And suppose that our random forces effectively have a standard deviation of say 1 meters per second squared.

sa = 1 # sigma a

The trajectory of the tennis ball will look something like below:

A REAL Tennis Ball Dropping (Image by Author)

Simulated with codes below:

def xk(F, xk1, G, sa):
    _xk = F.dot(xk1) + \
          G * np.random.normal(loc=0, scale=sa)    return _xk

Prediction

How accurate is our prediction with equations of motion?

Now suppose we try to predict the trajectory of the tennis ball with only the equations of motion, how accurate will it be?

Equations of Motion Predicted Trajectory vs Real (Image by Author)

As you can see, the difference can accumulate and become quite significant, in this case, they are off by more than 1000 meters.

Observation

How do we improve this prediction with Kalman filter?

Kalman filter takes advantage of the fact that you don’t need to wait till the tennis hits the ground to know its position. If you have a camera with you, for example, you can take a picture of the tennis ball every 10 seconds and estimate its position from it so that you can update your prediction from the observation. (So don’t ever forget to bring your phone!)

Suppose at time t, we take a picture of tennis ball, what will it look like?

Taking a Picture of the Tennis Ball at Time t(Image by Author)

It’s not going to give you a perfect observation (nothing can be perfect). But you can get a good estimate from it. Below is the mathematical representation of the observation:

Observed Tennis Ball Position at time t

In this case, you have the actual state of the ball for which you don’t know the exact values, but you can observe it with an observation model, H.

H represents our camera, since from the picture taken by a camera you can only find position y at time t, it’s a horizontal unit vector. If, say, we have a radar that can actually observe the velocity as well, we can use [1, 1, 0] as observation model.

The third part represents the inaccuracies associated with the observation, and it’s a normal random variable with mean 0 and standard deviation sigma z.

Kalman Filter

Apply Kalman filter to improve the predictions with observations.

Kalman filter provides us with a recipe for using the observation to improve the prediction.

You start with your predicted state(position, velocity, acceleration) of the tennis ball and their predicted covariances (accuracy in prediction), the predicted covariance is updated during each step along with state proportional to how much random force is on the tennis ball.

Calculate the difference between the predicted position and the observation.

Calculate the gain warranted from the difference based on the covariance of the tennis ball’s state and the accuracy of the observation.

— here the idea is that if the observation is perfect (R=0), then the gain should be 1, so that your updated estimate is basically the observation.

— if the variance of the position is high (the prediction is not very accurate), even if the observation is not that accurate, it’s a good idea to give a bit more weight to it since compared to the accuracy of the prediction it’s not so bad.

Apply the gain to the predicted position and predicted covariance to update them.

Results

How does Kalman filter perform?

As you can see, the purpose of Kalman filter is to improve your predictions with periodic observations. If you have observations, great, let’s use them, if not, just continue the prediction process.

Below is a comparison of performance using Kalman filters to improve prediction with observations every 50, 10, 5 seconds:

As you can see, more frequent observations definitely improve predicted values.

Conclusion

What’s next?

By going through this simple application of Kalman filter, I hope my readers have gained an intuitive understanding of the Kalman filter estimation improvement process. As we can see from the example, how accurate the prediction is ultimately depends on the model used to generate the prediction. Kalman filter can help improve the prediction with appropriately chosen error models for the prediction(sigma a) and the observation (sigma z). There are many improvements that can be made to the Kalman filter, we’ll talk about them in the future.