Extended Kalman filter

What it computes

The Kalman filter needs linear models, and a rover's aren't: where it goes depends on its heading, and a beacon's bearing depends on where it points. The extended Kalman filter (EKF) linearises each model around the current estimate, using its Jacobian, then runs the usual Kalman equations. Here it tracks the pose $x = [p_x, p_y, \theta]$ with covariance $P$.

Predict with odometry

The wheels report driving $\Delta s$ and turning $\Delta\theta$, with covariance $Q$. The rover moves along its average heading $\bar\theta = \theta + \Delta\theta/2$:

with $F = \partial f / \partial x$ (3×3) and $G = \partial f / \partial u$ (3×2), taken at the old $x$.

Update with beacons

A beacon at $b$ reads a range and a bearing, with covariance $R$. With $(d_x, d_y) = b - (p_x, p_y)$, the model is $h(x) = [\lVert d \rVert,\ \operatorname{atan2}(d_y, d_x) - \theta]$ and $H = \partial h / \partial x$ (2×3):

for id, r, b in z:              # one beacon at a time
    y = [r, b] - h(x)           # innovation: wrap y[1]!
    S = H @ P @ H.T + R
    K = P @ H.T @ inv(S)
    x = x + K @ y               # and wrap x[2]
    P = (I - K @ H) @ P

Tools

• rover.wrap(a) wraps angles into (−π, π]. Bearings of 3.13 and −3.13 are 0.02 rad apart, not 6.26, so wrap every angle difference, and θ itself.
• np.arctan2(dy, dx), np.linalg.inv(S) and @ for matrix products.

The program

The rover drives a figure-8 among three beacons, and its true pose is hidden. Ten times a second the program feeds your filter the odometry step and the beacon readings. It draws your estimate in orange, with its 2σ ellipse every second, and raw odometry in grey.

See it in context

This is the linear Kalman filter, linearised. Noisy Sensors & Kalman Filters builds that step by step.

Your task

Implement ekf_predict(x, P, u, Q) and ekf_update(x, P, z, beacons, R). Each returns the new (x, P). z has one row [id, range, bearing] per beacon seen, and beacons[id] is that beacon's position.