Two sources, one state

A signal that drifts and a signal that is noisy, and why either alone is worse than both.

U6.1State estimationUniversity25 min

Do this lesson in the simulator

Print it: worksheet (PDF) · answers (PDF) · as a web page

Guide: Kalman filters explained

The robot has two ways of knowing which way it is facing, and both are bad.

  • The gyro, integrated. Smooth, responsive, no noise worth mentioning over a second. Drifts without limit.
  • The fused heading from the IMU, imu()[0]. Noisy, wobbly, and it wanders a few degrees, but it does not run away.

One drifts and does not wobble. The other wobbles and does not run away. That is the setup for the oldest trick in sensor fusion.

The complementary filter

mine = a * (mine + rate * dt) + (1 - a) * fused

with a near 0.98. Read it as: mostly trust the gyro over the short term, and let the absolute reading pull you back, slowly, over the long term.

It is called complementary because the two weightings add to one, and in the frequency domain the two filters are complements: a high pass on the gyro, a low pass on the absolute source, summing to unity gain at every frequency. Each sensor is used where it is good and ignored where it is not.

The complementary filter: the gyro's turn rate moves the estimate on, the fused heading is wrapped and mixed in, and the two weights add to oneturn rateimu()[1]fused headingimu()[0]move onmine + rate × dtwrapwithin 180 of mine× 0.98× 0.02+minenext tickcompared with mineshort termlong term
Each tick, the turn rate moves the estimate on, and 0.98 of that is kept. The fused heading only ever gets 0.02 of a say, so its wobble is diluted, but it gets that say on every tick, so it slowly pulls any drift back out. The two weights add up to one.

Choosing a

The time constant is a * dt / (1 - a). At a = 0.98 and dt = 0.1 that is about 5 seconds: drift is pulled out over a few seconds, and the noise on the absolute reading is smoothed over the same span.

  • Too close to 1 and drift is not corrected fast enough.
  • Too far below and the noise comes straight through.
How much of a gap between the estimate and the fused heading is left, against time, at a = 0.980510150255075100time, secondsgap left, %4.9 s: 37% left9.8 s: 14%
Each tick keeps 0.98 of any gap between your estimate and the fused heading, so the gap shrinks by the same fraction every tick. After one time constant, 0.98 × 0.1 / 0.02 = 4.9 s, about a third of it is left, and after two about a seventh. Drift is pulled out on this timescale, and the fused heading's wobble is averaged over it.

Wrapping

Angles wrap at 360, and averaging 359 with 1 naively gives 180, which is exactly the wrong answer. Always bring the two numbers within half a turn of each other before mixing:

while fused - mine > 180:
    fused -= 360
while mine - fused > 180:
    fused += 360

This bug is a rite of passage. It shows up as an estimate that is fine until the robot crosses north, then stalls while the robot keeps turning. In the simulator it is half a turn out within 4 seconds, and anything steering by it is lost.

359 and 1 on a number line are far apart, but on the compass they are neighbours either side of north, and their plain average points southas plain numbers0901802703601359average 180halfway between them is 180on the compassESWmine 359fused 1northwrappedaverage 360plainaverage 180
Two headings a whisker either side of north. As plain numbers they are almost a whole turn apart, and their average, 180, points due south. The wrapping loops turn the fused 1 into 361, within half a turn of 359, and now the average of the two is 360, which is north, where both of them point.

Seeing all three

The three headings never differ by more than about 16 degrees, so plotted as headings on a chart that runs to 360 they sit on top of each other. To see the difference, plot each one's error instead: how far it is from heading(), the simulator's truth. A real robot has no truth to compare with, which is why the task does not allow heading(); here it is only used to draw the chart.

from bugbot import *
connect()

DT = 0.1
gyro = imu()[0]
mine = imu()[0]

def wrap(e):                  # an angle error, from -180 to 180
    return (e + 180) % 360 - 180

drive(0, 0, 40)
for i in range(300):          # 30 seconds
    if i == 60:
        stop()                # turn for 6 seconds, then sit still
    rate, fused = imu()[1], imu()[0]
    gyro += rate * DT
    f = fused
    while f - mine > 180: f -= 360
    while mine - f > 180: f += 360
    mine = 0.98 * (mine + rate * DT) + 0.02 * f
    truth = heading()         # the truth, for the chart only
    plot("gyro", wrap(gyro - truth))
    plot("fused", wrap(fused - truth))
    plot("mine", wrap(mine - truth))
    wait(DT)
stop()

Run this in the simulator

Three error lines. The gyro is smooth and slowly wrong: this robot's gyro reads about a quarter of a degree per second low, and by the end it is 5 degrees out and still going. The fused heading is hairy, anywhere from 3 degrees below the truth to 12 above. Yours is smooth and stays close: over the whole run its average error (the root mean square) is 2.0 degrees, against 3.2 for the gyro and 4.4 for the fused heading.

Task: a complementary filter

Turn the robot about, running a complementary filter on the heading, plotting gyro, fused and mine, and print my heading: at the end. heading() is the truth and is not allowed, so plot the three headings themselves. They will lie almost on top of each other, which is expected: they differ by a few degrees on a chart that runs to 360.

from bugbot import *
connect()

DT = 0.1
A = 0.98
mine = imu()[0]

Challenges

  1. Run it at a = 0.7 and a = 0.999 and describe both failures.
  2. Remove the wrapping and turn the robot through north. What happens?
  3. Compare your estimate with heading() afterwards. Which is better, yours or imu()[0] alone?