The Extended Kalman Filter (EKF)

A robot that turns, and sensors that measure range and bearing, are not linear, so the plain Kalman filter does not apply. The extended Kalman filter linearizes the models around the current estimate at every step, using their Jacobians, and then runs the usual Kalman equations.

Odometry noise Range noise σ Bearing noise σ Use landmark readings

Linearize, then filter

Motion $\mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k)$ and the range-bearing reading $\mathbf{z} = h(\mathbf{x})$ to a landmark at $\mathbf{m}$ are nonlinear. The EKF uses their Jacobians $F = \partial f/\partial\mathbf{x}$ and $H = \partial h/\partial\mathbf{x}$:

$$h(\mathbf{x}) = \begin{bmatrix} \sqrt{(m_x - x)^2 + (m_y - y)^2} \\ \operatorname{atan2}(m_y - y,\ m_x - x) - \theta \end{bmatrix} \qquad P^- = F P F^\top + Q, \quad K = P^- H^\top (H P^- H^\top + R)^{-1}$$

What to notice

With landmark readings off, the ellipse grows without limit, since odometry alone drifts. Turn them on and the ellipse stays bounded, shrinking most in the direction the landmark constrains. Range pins the distance and bearing pins the angle, so the ellipse is often long in one direction. The EKF works well when errors are small. With big bearing noise its linearization gets poor and the estimate can become overconfident, which the UKF handles better.