Problems · Problem 9 · Estimation · Medium

Kalman filter

Predict with a motion model, correct with each measurement, and keep track of how uncertain you are.

Predict / updateCovarianceKalman gain

Builds on Combining two sensors, from the free Foundations.

Write and run this problem in the simulator with Pro

What it computes

A Kalman filter estimates a hidden state $x$ (say, position and velocity) from noisy measurements $z$ . It keeps a Gaussian belief: the mean $x$ and the covariance $P$ . It is the exact Bayesian answer when the models are linear and the noise is Gaussian.

Two steps, repeated

Predict with the motion model $x_{k+1} = F x_k + w$ , where $w \sim \mathcal{N}(0, Q)$ :

x \leftarrow F x, \qquad P \leftarrow F P F^T + Q

Update with a measurement $z = H x + v$ , where $v \sim \mathcal{N}(0, R)$ :

\begin{aligned} y &= z - H x && \text{innovation} \\ S &= H P H^T + R && \text{its covariance} \\ K &= P H^T S^{-1} && \text{Kalman gain} \\ x &\leftarrow x + K y, \qquad P \leftarrow (I - K H) P \end{aligned}

Prediction grows the uncertainty and each update shrinks it. $K$ weighs the two: a precise sensor (small $R$ ) pulls the estimate towards the measurement, and a confident belief (small $P$ ) keeps it where it was.

The program

A conveyor slides the red ball across the table. The sensor reports its position 10 times a second with σ = 1 cm. The program tracks $x = [p_x, p_y, v_x, v_y]$ with a constant-velocity model. It plots the readings against your estimate, and records both in track.

See it in context

Noisy Sensors & Kalman Filters builds this filter step by step, then uses it to catch the ball.

Your task

Implement kf_predict(x, P, F, Q) and kf_update(x, P, z, H, R). Each returns the new (x, P), and each must work for any state size. The grader tests both on random matrices, then checks that on the live conveyor your estimates are much closer to the ball than the raw readings.

Two steps, repeated

Predict with the motion model

x_{k+1} = F x_k + w

, where

w \sim \mathcal{N}(0, Q)

x \leftarrow F x, \qquad P \leftarrow F P F^T + Q

Update with a measurement

z = H x + v

, where

v \sim \mathcal{N}(0, R)

\begin{aligned} y &= z - H x && \text{innovation} \\ S &= H P H^T + R && \text{its covariance} \\ K &= P H^T S^{-1} && \text{Kalman gain} \\ x &\leftarrow x + K y, \qquad P \leftarrow (I - K H) P \end{aligned}

Prediction grows the uncertainty and each update shrinks it.

K

weighs the two: a precise sensor (small

R

) pulls the estimate towards the measurement, and a confident belief (small

P

) keeps it where it was.

The program

A conveyor slides the red ball across the table. The sensor reports its position 10 times a second with σ = 1 cm. The program tracks

x = [p_x, p_y, v_x, v_y]

with a constant-velocity model. It plots the readings against your estimate, and records both in track.

Kalman filter

What it computes

Two steps, repeated

The program

See it in context

Your task

This problem is part of CrackRobotics Pro

Kalman filter

What it computes

Two steps, repeated

The program

See it in context

Your task

This problem is part of CrackRobotics Pro