An EKF with beacons

One Gaussian instead of many cells

In 2-D, with a heading, a histogram would need millions of cells. When you roughly know where you are, a Gaussian is enough: a best guess $x = [p_x, p_y, \theta]$ and a 3×3 covariance $P$ saying how unsure it is. The extended Kalman filter (EKF) keeps both up to date.

Predict with the wheels

Move $x$ with step 1's move. The old uncertainty is carried along and the increment's own noise, with covariance $Q$, adds to it:

$F = \partial f / \partial x$ and $G = \partial f / \partial u$ are the Jacobians of the motion $f$: its partial derivatives. With $\bar\theta = \theta + \Delta\theta/2$,

and $G$'s two columns are the derivatives of $f$ by $\Delta s$ and by $\Delta\theta$. Remember that $\bar\theta$ depends on $\Delta\theta$.

Correct with a beacon

A beacon at $b$ reads a range and a bearing. With $d = b - [p_x, p_y]$ and $q = d_x^2 + d_y^2$, the reading you expect and its Jacobian are

The innovation $y = z - h$ is the surprise (wrap its bearing). The gain $K$ decides how much of it to believe, given the reading's noise covariance $R$:

A large $P$ trusts the beacon; a small one trusts the prediction. Noisy Sensors & Kalman Filters builds this update in one dimension.

The program

The rover laps a room with four beacons but only sees those within 25 cm (blue circles). Your estimate is orange, raw odometry grey, and every half second your 2σ ellipse is drawn, five times bigger. It grows while no beacon is in sight and shrinks at each sighting.

Your task

Write motion_jacobians(x, u) → (F, G), ekf_predict(x, P, u, Q) → (x, P), observe(x, b) → (h, H) and ekf_update(x, P, z, b, R) → (x, P), wrapping every angle. The grader checks each on fixed cases, then your estimate and your ellipse against the rover's true path.