The Kalman Filter in 1D

A Kalman filter keeps a best guess and an uncertainty. It predicts where the robot moved, which makes it less sure, then corrects with a sensor reading, which makes it more sure.

Sensor noise (σ, m) Process noise (trust in model) Readings per second

The two steps

Predict where the robot moved, which grows the uncertainty:

$$\begin{aligned} \hat{\mathbf{x}}^- &= F\,\hat{\mathbf{x}} \\ P^- &= F P F^\top + Q \end{aligned}$$

Update with a reading $z$, which shrinks the uncertainty:

$$\begin{aligned} K &= P^- H^\top \left(H P^- H^\top + R\right)^{-1} \\ \hat{\mathbf{x}} &= \hat{\mathbf{x}}^- + K\left(z - H\hat{\mathbf{x}}^-\right) \\ P &= (I - K H)\,P^- \end{aligned}$$

The state is $\hat{\mathbf{x}} = [\,p,\ v\,]^\top$, position and velocity. With time step $\Delta t$, the motion model and the sensor model are below. $H$ picks out position only, since that is all the sensor reads.

$$F = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \qquad H = \begin{bmatrix} 1 & 0 \end{bmatrix}$$

What to notice

The shaded band is the filter's uncertainty (two standard deviations). It grows between readings and shrinks at each one. The Kalman gain $K$ decides how much to trust a reading. Raise the sensor noise and $K$ drops, so the estimate gets smoother but slower to react. Raise the process noise and the filter trusts its model less, so it follows the readings more closely.

The filter here tracks both position and velocity, even though it only ever measures position. That is a key strength of the Kalman filter: it can estimate things the sensor cannot see directly.