Kalman filters for autonomous robots

Overview

A practical introduction to the Kalman filter for autonomous robots. Most tutorials lean on heavy mathematics and give few numerical examples, so this one builds the filter from its five matrices and runs it in small simulations that show what each step does.

How it works

The state x holds four values, and the uncertainty matrix P starts from the initial uncertainty of each one. Every step has two parts. Predict moves the state forward with the state transition matrix F, so that x(t+1) = F·x(t), and lets the uncertainty grow. Update takes a measurement of two values, which the measurement matrix H maps to the state, weighs it by its own uncertainty R, and pulls the estimate toward it in proportion to how far each side can be trusted.

The simulations

A one-dimensional simulation comes first. A two-dimensional one follows a car that drives a route or a circle while the filter estimates where it is, and a third uses the filter’s prediction to tell whether the car will stop before a red light. Each run is animated with Matplotlib, and a video linked from the repository shows the result.