ROBOTICS 101Concept explainer

The Kalman Filter in 2D

In 2D the Kalman filter tracks position and velocity in both directions at once, and its uncertainty becomes an ellipse. Between readings the ellipse grows, stretched along the direction of motion. Each reading shrinks it again.

State and model

The state is \(\mathbf{x} = (p_x, p_y, v_x, v_y)\) with a constant-velocity model, and readings give position only:

$$F = \begin{bmatrix} I & \Delta t\,I \\ 0 & I \end{bmatrix} \qquad H = \begin{bmatrix} I & 0 \end{bmatrix} \qquad P^- = FPF^\top + Q, \quad K = P^-H^\top(HP^-H^\top + R)^{-1}$$

What to notice

Between readings the red ellipse grows, more along the direction the robot is moving, because velocity uncertainty turns into position uncertainty along that direction. Each reading, a black cross, snaps the ellipse smaller. Fewer readings per second mean bigger ellipses. The filter also estimates velocity, shown by the arrow, though it never measures velocity directly.