Extended Kalman filter explained

Why a straight Kalman filter cannot take a range or a bearing, what linearising means, and what the Jacobian is, with all four entries of a 2 by 2 written out by hand. Watch an EKF fix a robot's position from one tag, and watch a bad first guess throw the same filter 336 cm off. Every demo runs in the browser.

Guidefree, runs in your browser

An extended Kalman filter, or EKF, is the version of the Kalman filter you use when the sums are not straight lines. Every robot that works out where it is from a landmark needs one, because a distance and an angle to a landmark are not straight lines in the robot's position. It is the filter in most drone flight controllers, in a lot of self-driving cars, and in nearly every robot that carries a camera and a map. On this page a small robot drives across a two metre mat with one tag on the wall, measures how far away the tag is and which way it lies, and works out where it is. Each demo below is a real program you can change and run.

The robot knows which way it is facing, so the filter has only two things to work out: how far east it is, and how far north. Everything below is done with two numbers and a 2 by 2, written out by hand.

Why the straight filter cannot take this reading

A Kalman filter needs a measurement it can write as a straight line in what it is estimating:

reading = h1 × x + h2 × y + (a constant)

That is what lets it compare a prediction with a reading and work out the gain. A gap measured by a distance sensor pointing at a wall is like that. A tag is not. If the tag is at (tx, ty) and the robot is at (x, y):

range   = square root of ((tx - x)² + (ty - y)²)
bearing = atan2(tx - x, ty - y) - heading

There is no way to write a square root or an atan2 as h1 × x + h2 × y. Lines of equal range are circles round the tag, and lines of equal bearing are spokes going out from it. A straight filter can only draw straight lines.

This demo drives the robot across the mat and plots both readings against what a straight line through the starting point would have predicted.

from bugbot import *
import math
connect()

TAG_X, TAG_Y = 40.0, 160.0    # where tag 20 is, in cm on the mat
START = (60.0, 50.0)          # where the robot starts
CAM_F = 92.4                  # the camera's focal length, in pixels
DEG = 180 / math.pi

# the straight line through the start: the range and bearing there, and how much
# each of them changes when the robot moves one centimetre east or north
ex, ey = TAG_X - START[0], TAG_Y - START[1]
r0 = math.hypot(ex, ey)
b0 = math.degrees(math.atan2(ex, ey))
dr_dx, dr_dy = -ex / r0, -ey / r0
db_dx, db_dy = -DEG * ey / (r0 * r0), DEG * ex / (r0 * r0)
print("at the start: range", round(r0, 1), "cm, bearing", round(b0, 1), "degrees")
print("one cm east: range", round(dr_dx, 3), "cm, bearing", round(db_dx, 3), "degrees")
print("one cm north: range", round(dr_dy, 3), "cm, bearing", round(db_dy, 3), "degrees")

set_cv("apriltag")
drive(50, 25, 0)
for tick in range(60):
    tags = [t for t in apriltags() if t[0] == 20]
    px, py = position()
    if tags:
        plot("range", tags[0][3])
        plot("range, straight line", r0 + dr_dx * px + dr_dy * py)
        seen = math.degrees(math.atan2(tags[0][1] - 160, CAM_F)) + heading()
        plot("bearing", (seen + 180) % 360 - 180)
        plot("bearing, straight line", b0 + db_dx * px + db_dy * py)
    wait(0.1)
stop()
print("moved", round(px, 1), "east and", round(py, 1), "north")

The two lines start together and peel apart. By the end of the run the straight line is 7 cm out on the range and nearly 10 degrees out on the bearing. A filter fed those predictions would believe the robot is somewhere it is not, and, worse, it would be sure of it.

Look at the four numbers the program prints. dr_dx is how much the range changes for one centimetre east: 0.179 cm. db_dx is how much the bearing changes for the same centimetre: -0.504 degrees. Those four numbers are the line, and they only hold near the place they were worked out.

Linearising, and the Jacobian

Linearising means replacing a curve with the straight line that touches it where you are: the tangent. The extended Kalman filter does exactly that, and it does it again at every step, at wherever the estimate has got to.

The four numbers above have a name. Written as a table, one row per reading and one column per part of the state, they are the Jacobian:

        how much it changes per cm east   per cm north
range        -(tx - x) / r                  -(ty - y) / r
bearing      -(ty - y) / r²                  (tx - x) / r²

In words: move a centimetre straight towards the tag and the range falls by a centimetre, and the bearing does not change at all. Move a centimetre sideways and the range hardly changes, but the bearing swings, and it swings more the closer you are, which is what the r² says. The bearing row here is multiplied by 180/π, because the bearing is in degrees and the calculus gives radians.

That is the whole difference between a Kalman filter and an extended one. The ordinary filter is handed an H that never changes. The extended one works H out from the current estimate every time a reading arrives, and uses the real curved sum, not the line, to predict what the reading should have been.

The EKF working

Here is the filter. Two states, so the uncertainty P is a 2 by 2: p11 is how unsure it is about east, p22 about north, and p12 says how much the two errors go together. The prediction comes from the optical flow sensor underneath the robot. The correction takes the two readings one at a time, which keeps each one a scalar update exactly like the one in the Kalman filter guide, with p / (p + R) grown up into two dimensions.

The program runs two filters side by side. The EKF works the Jacobian out again at every reading. The straight one works it out once, at the first guess, and keeps it. Both start from the same place, both get the same readings, and both predict the same way.

from bugbot import *
import math
connect()

# change these and press Run
X0, Y0 = 60.0, 50.0      # the filter's first guess at where the robot is
P0 = 100.0               # how unsure it is of that guess, in cm squared

TAG_X, TAG_Y = 40.0, 160.0   # where tag 20 is, in cm on the mat
START = (60.0, 50.0)         # where the robot really starts
EVERY = 10                   # ticks between tag readings: the detector runs once a second
R_RANGE = 0.25               # how noisy the tag range is, cm squared
R_BEARING = 0.4              # how noisy the tag bearing is, degrees squared
Q = 0.02                     # how far the prediction can slip in one step
DT = 0.1
CAM_F = 92.4                 # the camera's focal length, in pixels
DEG = 180 / math.pi


def wrap(a):
    return (a + 180) % 360 - 180


def readings(x, y):
    """What the tag would look like from (x, y), and how fast each reading changes
    when x and y change: the two rows of the Jacobian, worked out by hand."""
    ex, ey = TAG_X - x, TAG_Y - y
    r = math.hypot(ex, ey)
    return (r, wrap(math.degrees(math.atan2(ex, ey))),
            (-ex / r, -ey / r),                              # range, per cm east and north
            (-DEG * ey / (r * r), DEG * ex / (r * r)))       # bearing, per cm east and north


class Filter:
    def __init__(self):
        self.x, self.y = X0, Y0
        self.p11, self.p12, self.p22 = P0, 0.0, P0

    def predict(self, step_x, step_y):
        self.x, self.y = self.x + step_x, self.y + step_y
        self.p11, self.p22 = self.p11 + Q, self.p22 + Q

    def correct(self, h1, h2, innovation, r):
        """One number from a sensor. h1 and h2 are the row of the Jacobian for it."""
        a1 = self.p11 * h1 + self.p12 * h2          # P times h
        a2 = self.p12 * h1 + self.p22 * h2
        s = h1 * a1 + h2 * a2 + r                   # how unsure the predicted reading is
        k1, k2 = a1 / s, a2 / s                     # the gain: one number per state
        self.x = self.x + k1 * innovation
        self.y = self.y + k2 * innovation
        self.p11 = self.p11 - k1 * a1
        self.p12 = self.p12 - k1 * a2
        self.p22 = self.p22 - k2 * a2


ekf = Filter()
straight = Filter()
# the straight filter works its line out once, at the first guess, and keeps it for ever
r0, b0, hr0, hb0 = readings(X0, Y0)

set_cv("apriltag")
drive(50, 25, 0)
for tick in range(60):
    # predict: both filters move the same way, by what the flow sensor saw
    h = math.radians(heading())
    side, ahead = flow()
    step_x = (side * math.cos(h) + ahead * math.sin(h)) * DT
    step_y = (-side * math.sin(h) + ahead * math.cos(h)) * DT
    ekf.predict(step_x, step_y)
    straight.predict(step_x, step_y)
    tags = [t for t in apriltags() if t[0] == 20] if tick % EVERY == 0 else []
    if tags:
        z_range = tags[0][3]
        z_bearing = wrap(math.degrees(math.atan2(tags[0][1] - 160, CAM_F)) + heading())
        # the EKF: work the line out again, at the estimate, for every reading
        r, b, hr, hb = readings(ekf.x, ekf.y)
        ekf.correct(hr[0], hr[1], z_range - r, R_RANGE)
        r, b, hr, hb = readings(ekf.x, ekf.y)
        ekf.correct(hb[0], hb[1], wrap(z_bearing - b), R_BEARING)
        # the straight filter: the line from the start, and a reading predicted along it
        guess_r = r0 + hr0[0] * (straight.x - X0) + hr0[1] * (straight.y - Y0)
        straight.correct(hr0[0], hr0[1], z_range - guess_r, R_RANGE)
        guess_b = b0 + hb0[0] * (straight.x - X0) + hb0[1] * (straight.y - Y0)
        straight.correct(hb0[0], hb0[1], wrap(z_bearing - guess_b), R_BEARING)
    px, py = position()
    true_x, true_y = START[0] + px, START[1] + py
    plot("EKF off by", math.hypot(ekf.x - true_x, ekf.y - true_y))
    plot("straight off by", math.hypot(straight.x - true_x, straight.y - true_y))
    draw("EKF", [(ekf.x, ekf.y)], "green", "squares", 5)
    draw("truth", [(true_x, true_y)], "white", "squares", 3)
    wait(DT)
stop()
print("truth   ", round(true_x, 1), round(true_y, 1))
print("EKF     ", round(ekf.x, 1), round(ekf.y, 1))
print("straight", round(straight.x, 1), round(straight.y, 1))

On the mat the green square is the EKF's estimate and the small white one is the truth, and for most of the run you cannot see the green one because the white one is on top of it. The tag is read once a second, so between readings both filters are running on the flow sensor alone.

Three things are worth reading closely in the code.

• The innovation is worked out with the real sum, not the line. z_range - r uses the square root. The Jacobian is only used to decide how to split the correction between east and north.
• The bearing innovation is wrapped. wrap(z_bearing - b) turns 350 degrees into -10. An EKF that forgets this is the classic bug: one reading either side of north and the filter is thrown hundreds of centimetres.
• The two readings are taken one at a time. Nothing here inverts a matrix. Feeding the range first and then the bearing, each with its own row, gives the same answer as doing both at once when their noises are unrelated, and it is much easier to read.

Try EVERY = 1, a tag read ten times a second: the EKF sits within 0.4 cm the whole way, while the straight filter, given ten times as many readings it cannot use properly, ends 14.9 cm out. Try EVERY = 40, a reading every four seconds: the EKF wanders to 1.7 cm between fixes, which is the flow sensor drifting.

When the first guess is bad

The tangent is only a good stand-in for the curve near the place it was drawn. If the estimate is a long way from the truth, the Jacobian is worked out at the wrong place, and the correction it produces can point the wrong way.

Here is the same program, with one change: the filter starts 200 cm north of where the robot really is.

from bugbot import *
import math
connect()

# change these and press Run
X0, Y0 = 60.0, 250.0     # the first guess: 200 cm from where the robot really is
P0 = 100.0               # how unsure it is of that guess, in cm squared

TAG_X, TAG_Y = 40.0, 160.0   # where tag 20 is, in cm on the mat
START = (60.0, 50.0)         # where the robot really starts
EVERY = 10                   # ticks between tag readings: the detector runs once a second
R_RANGE = 0.25               # how noisy the tag range is, cm squared
R_BEARING = 0.4              # how noisy the tag bearing is, degrees squared
Q = 0.02                     # how far the prediction can slip in one step
DT = 0.1
CAM_F = 92.4                 # the camera's focal length, in pixels
DEG = 180 / math.pi


def wrap(a):
    return (a + 180) % 360 - 180


def readings(x, y):
    ex, ey = TAG_X - x, TAG_Y - y
    r = math.hypot(ex, ey)
    return (r, wrap(math.degrees(math.atan2(ex, ey))),
            (-ex / r, -ey / r),
            (-DEG * ey / (r * r), DEG * ex / (r * r)))


class Filter:
    def __init__(self):
        self.x, self.y = X0, Y0
        self.p11, self.p12, self.p22 = P0, 0.0, P0

    def predict(self, step_x, step_y):
        self.x, self.y = self.x + step_x, self.y + step_y
        self.p11, self.p22 = self.p11 + Q, self.p22 + Q

    def correct(self, h1, h2, innovation, r):
        a1 = self.p11 * h1 + self.p12 * h2
        a2 = self.p12 * h1 + self.p22 * h2
        s = h1 * a1 + h2 * a2 + r
        k1, k2 = a1 / s, a2 / s
        self.x = self.x + k1 * innovation
        self.y = self.y + k2 * innovation
        self.p11 = self.p11 - k1 * a1
        self.p12 = self.p12 - k1 * a2
        self.p22 = self.p22 - k2 * a2


ekf = Filter()
set_cv("apriltag")
drive(50, 25, 0)
for tick in range(60):
    h = math.radians(heading())
    side, ahead = flow()
    ekf.predict((side * math.cos(h) + ahead * math.sin(h)) * DT,
                (-side * math.sin(h) + ahead * math.cos(h)) * DT)
    tags = [t for t in apriltags() if t[0] == 20] if tick % EVERY == 0 else []
    if tags:
        z_range = tags[0][3]
        z_bearing = wrap(math.degrees(math.atan2(tags[0][1] - 160, CAM_F)) + heading())
        r, b, hr, hb = readings(ekf.x, ekf.y)
        print(round(clock(), 1), "reading", round(z_range), "cm at", round(z_bearing),
              "degrees;  the filter expected", round(r), "cm at", round(b), "degrees")
        ekf.correct(hr[0], hr[1], z_range - r, R_RANGE)
        r, b, hr, hb = readings(ekf.x, ekf.y)
        ekf.correct(hb[0], hb[1], wrap(z_bearing - b), R_BEARING)
    px, py = position()
    true_x, true_y = START[0] + px, START[1] + py
    plot("EKF off by", math.hypot(ekf.x - true_x, ekf.y - true_y))
    draw("EKF", [(ekf.x, ekf.y)], "green", "squares", 5)
    draw("truth", [(true_x, true_y)], "white", "squares", 3)
    wait(DT)
stop()
print("truth", round(true_x, 1), round(true_y, 1), " EKF", round(ekf.x, 1), round(ekf.y, 1))

Read the printed lines. The first reading says the tag is 112 cm away at a bearing of -10 degrees. The filter, sitting 90 cm the far side of the tag, expects 92 cm at a bearing of -167 degrees. That innovation of 157 degrees is put through a Jacobian worked out at the wrong place, and the estimate is thrown to 336 cm from the truth, further away than it started. A second later the filter expects the tag to be 326 cm away when it is 106.

It does recover, slowly, because this tag gives an accurate reading every second and the geometry keeps changing. That is luck, not design. Give the same filter a noisier sensor, or a tag that goes out of view for a few seconds, and there is nothing to pull it back. This is what people mean when they say an EKF diverges: not that the arithmetic breaks, but that the estimate walks away from the truth while the filter's own P keeps shrinking, so it becomes more and more sure of an answer that is wrong.

Starting from a reading instead of a guess

The fix for a bad first guess is not to guess. A range and a bearing to a tag whose position you know say exactly where you are, so the first reading can be turned straight into a position, and the filter starts there.

from bugbot import *
import math
connect()

# change these and press Run
X0, Y0 = 60.0, 250.0     # the first guess is still 200 cm out
FIX_FIRST = True         # but work the first estimate out from the first reading

TAG_X, TAG_Y = 40.0, 160.0
START = (60.0, 50.0)
EVERY = 10
R_RANGE = 0.25
R_BEARING = 0.4
Q = 0.02
DT = 0.1
CAM_F = 92.4
DEG = 180 / math.pi

x, y = X0, Y0
p11, p12, p22 = 100.0, 0.0, 100.0
started = False


def wrap(a):
    return (a + 180) % 360 - 180


def correct(h1, h2, innovation, r):
    global x, y, p11, p12, p22
    a1 = p11 * h1 + p12 * h2
    a2 = p12 * h1 + p22 * h2
    s = h1 * a1 + h2 * a2 + r
    k1, k2 = a1 / s, a2 / s
    x = x + k1 * innovation
    y = y + k2 * innovation
    p11 = p11 - k1 * a1
    p12 = p12 - k1 * a2
    p22 = p22 - k2 * a2


set_cv("apriltag")
drive(50, 25, 0)
for tick in range(60):
    h = math.radians(heading())
    side, ahead = flow()
    x = x + (side * math.cos(h) + ahead * math.sin(h)) * DT
    y = y + (-side * math.sin(h) + ahead * math.cos(h)) * DT
    p11, p22 = p11 + Q, p22 + Q
    tags = [t for t in apriltags() if t[0] == 20] if tick % EVERY == 0 else []
    if tags:
        z_range = tags[0][3]
        z_bearing = wrap(math.degrees(math.atan2(tags[0][1] - 160, CAM_F)) + heading())
        if FIX_FIRST and not started:
            # a range and a bearing to a tag you know say exactly where you are
            x = TAG_X - z_range * math.sin(math.radians(z_bearing))
            y = TAG_Y - z_range * math.cos(math.radians(z_bearing))
            p11, p12, p22 = 4.0, 0.0, 4.0
            started = True
            print(round(clock(), 1), "started from the first reading at",
                  round(x, 1), round(y, 1))
        else:
            ex, ey = TAG_X - x, TAG_Y - y
            r = math.hypot(ex, ey)
            correct(-ex / r, -ey / r, z_range - r, R_RANGE)
            ex, ey = TAG_X - x, TAG_Y - y
            rsq = ex * ex + ey * ey
            guess = wrap(math.degrees(math.atan2(ex, ey)))
            correct(-DEG * ey / rsq, DEG * ex / rsq, wrap(z_bearing - guess), R_BEARING)
    px, py = position()
    true_x, true_y = START[0] + px, START[1] + py
    plot("off by", math.hypot(x - true_x, y - true_y))
    draw("EKF", [(x, y)], "green", "squares", 5)
    wait(DT)
stop()
print("filter", round(x, 1), round(y, 1), " truth", round(true_x, 1), round(true_y, 1))

Set FIX_FIRST = False and the same run is the diverging one from before. Every real system does something like this: a GPS receiver takes its first fix cold, a robot with tags looks for one before it moves, and a filter that has lost its way is often best restarted from a reading rather than nursed back.

Where an EKF goes wrong

• The first guess is too far out. As above. Start from a measurement where you can, and keep P honestly large until the filter has seen enough to earn a small one.
• It becomes sure of the wrong answer. Every correction shrinks P, whether or not the estimate is right. A filter with a tiny P ignores the readings that would have saved it. The usual guards are a floor under P, a larger Q, and throwing away readings whose innovation is far bigger than S says it should be.
• Angles. Every difference between two angles must be wrapped into -180 to 180, and an angle in the state has to be wrapped after every update as well.
• The curve is too bent to linearise. Close to the tag, the bearing's Jacobian goes as 1 / r², so at 5 cm it is 400 times what it is at a metre. A tangent drawn there is wrong almost immediately.
• The wrong landmark. If the robot matches a reading to the wrong tag, the filter has no way to tell. This is called data association, and on a real robot it causes more failures than the linearising does.
• It assumes one bell curve. A robot that could be in either of two identical corridors has a belief with two humps, and no EKF can hold that. A particle filter can.

When the bending is too strong, the usual next step is an unscented Kalman filter (UKF), which pushes a handful of carefully chosen sample points through the real curved sum instead of drawing a tangent. It costs a little more and needs no Jacobian at all, which is why it is popular where the maths is awkward to differentiate.

Where this is taught

• Predict and correct and The Kalman gain build the one-dimensional filter this page extends.
• Q and R is where the two noise numbers come from, and how to measure them.
• Covariance and the ellipse is what p11, p12 and p22 mean together.
• Fusing a fix and Project: navigate on the estimate use a tag as the measurement, as this page does.
• A tag as a fix and Intrinsics and calibration are where the range and the bearing come from, and why the focal length is 92.4 pixels.
• A fix from a landmark is the same job without a filter, and shows what the filter is buying.
• The Kalman filter guide covers the straight filter, and the particle filter guide covers what to do when one bell curve is not enough.

Questions

What is an extended Kalman filter?

It is a Kalman filter for a system whose prediction or measurement is not a straight line in the thing being estimated. At every step it replaces the curve with the tangent at the current estimate, using derivatives, and then runs the ordinary Kalman arithmetic on that line. The innovation is still worked out with the real curved sum, so only the splitting of the correction uses the approximation.

Why can a Kalman filter not use a range and a bearing?

Because the ordinary filter needs reading = H × state, with H a fixed table of numbers. A range is a square root of squares and a bearing is an atan2, and neither can be written that way. Lines of equal range are circles round the landmark and lines of equal bearing are spokes, and a straight filter can only draw straight lines across them.

What is a Jacobian in a Kalman filter?

A table with one row per reading and one column per part of the state, holding how much that reading changes when that part of the state changes by one. For a range and bearing to a tag it is a 2 by 2, and the demos on this page write out all four entries by hand. It is the multi-dimensional version of a gradient, and it is the tangent the filter uses in place of the curve.

What is the difference between a Kalman filter and an extended Kalman filter?

The ordinary filter has a fixed F and H, works exactly, and is the best possible estimator when the system really is linear and the noise really is Gaussian. The extended filter works out F and H again at every step from the current estimate, is an approximation, and can diverge if the estimate is far from the truth. Everything else, the predict step, the gain, the shrinking of P, is the same.

Why does an extended Kalman filter diverge?

Because the tangent is only right near the place it was drawn. If the estimate is far from the truth, the Jacobian is worked out at the wrong place and the correction can point the wrong way, as the third demo on this page shows: a first correction that takes the error from 200 cm to 336. It gets worse if P has already shrunk, because then the filter trusts itself more than the readings that would fix it.

How do you stop an EKF diverging?

Start it from a measurement rather than a guess. Keep P large until it has earned being small, and put a floor under it. Wrap every angle. Check each innovation against S and throw away the ones that are far too big to be true. If the model is very bent, use an unscented Kalman filter or a particle filter instead.

What is the innovation in a Kalman filter?

The difference between the reading you got and the reading the filter expected. In an EKF it is worked out with the real model, for example measured range - the square root at the estimate, and its size is a good health check: it should be small compared with the square root of S, the filter's own idea of how uncertain that prediction was.

What is an unscented Kalman filter?

A filter that avoids Jacobians by picking a small set of sample points, called sigma points, spread around the estimate to match its uncertainty, pushing each one through the real model, and fitting a new mean and covariance to what comes out. It handles bending better than an EKF and needs no derivatives, at the cost of a few more model evaluations per step.

Do self-driving cars use an EKF?

They use filters of this family for fusing wheel odometry, inertial sensors, GPS and landmarks, usually an EKF or a UKF, often several running on different parts of the problem. Tracking other vehicles and pedestrians is usually done with filters of the same shape, one per object, plus the data association that decides which measurement belongs to which object.

Learn it step by step

These lessons build the same ideas one at a time, each with tasks the simulator marks.

1. U6.1 Two sources, one state State estimation, University
2. U6.2 Predict and correct State estimation, University
3. U6.3 The Kalman gain State estimation, University
4. U6.4 Q and R State estimation, University
5. U6.5 Covariance and the ellipse State estimation, University
6. U6.6 Fusing a fix State estimation, University
7. U6.7 Project: navigate on the estimate State estimation, University
8. U3.5 A fix from a landmark Odometry and drift, University
9. U11.2 Intrinsics and calibration Vision, University
10. U11.4 A tag as a fix Vision, University