Covariance and the ellipse

Uncertainty in two dimensions has a shape, and the shape is the useful part.

U6.5State estimationUniversity30 min

Do this lesson in the simulator

In one dimension, uncertainty is a number. In two, it is a shape, and the shape carries information the number cannot.

Why it is not a circle

Drive a metre in a straight line. Two errors are growing, and they are not the same size.

  • Along the direction of travel: scale error. A few percent of a metre, so a few centimetres.
  • Across it: heading error times distance. Half a degree over a metre is about 1 cm.

So the uncertainty is an ellipse with its long axis along the path. Drive a different direction and the ellipse turns with you. After a turn it is a mess of both.

from bugbot import *
import math
connect()

DT = 0.1
along = across = 0.0
head_sigma = 0.5

forward(70)
for i in range(50):
    stepped = abs(flow()[1]) * DT
    head_sigma += 0.3 * DT
    along += 0.04 * stepped
    across += stepped * math.radians(head_sigma)
    plot("along", along)
    plot("across", across)
    wait(DT)
stop()
print("after", round(along / 0.04, 0), "cm: along", round(along, 2), "across", round(across, 2))

Run this in the simulator

Two lines climbing at different rates. That is the ellipse stretching.

The covariance matrix

For a state of (x, y) the uncertainty is

P = [ var(x)      cov(x,y) ]
    [ cov(x,y)    var(y)   ]

The diagonal is how uncertain each axis is. The off-diagonal is whether the errors move together, and it is the interesting one: it is what makes the ellipse tilt rather than sitting square to the axes.

The eigenvectors of P are the ellipse's axes and the square roots of its eigenvalues are their lengths. One sigma covers about 39 percent of the probability in two dimensions, not 68 as in one, which surprises everyone the first time. The 95 percent ellipse is about 2.45 sigma.

Why this earns its keep

Fixes are directional. A tag straight ahead measures range well and bearing poorly, so it shrinks the ellipse along the line of sight and hardly touches it sideways. A filter carrying the full matrix takes exactly the right amount from that fix. A filter carrying one number cannot.

It tells you where to go. If the ellipse is long across the track, you need a landmark to the side, not one ahead. Planning a route so that the uncertainty stays small is a real field, called active perception, and it starts with knowing the shape.

It tells you when to stop trusting yourself. A growing ellipse is the filter saying, honestly, that it needs a fix. A robot that watches its own covariance and goes looking for a landmark when it grows too large is doing something quite sophisticated with very little code.

In practice

The full Extended Kalman Filter carries P through the nonlinear motion model with Jacobians, and that is a term's work on its own. The value of doing it by hand once, in two dimensions, with an along and an across number, is that it makes the matrix version readable when you meet it.

Task: the shape of the uncertainty

Drive at least 60 cm, growing an along-track and an across-track uncertainty as you go. Plot both, and print along: and across: at the end.

from bugbot import *
import math
connect()

DT = 0.1
along = across = 0.0
head_sigma = 0.5

Challenges

  1. Turn 90 degrees half way and swap the two, since along and across have changed places in the world.
  2. Work out how far the robot can drive before the across-track uncertainty passes 10 cm.
  3. Given a tag directly ahead, which of the two would a fix shrink?