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

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, but it does not run away.

One drifts and does not wobble. The other wobbles and does not drift. 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.

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.

Wrapping

Angles wrap at 360, and blending 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 a robot that is fine until it crosses north, then spins on the spot.

Seeing all three

from bugbot import *
connect()

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

drive(0, 0, 40)
for i in range(60):
    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
    plot("gyro", gyro % 360)
    plot("fused", fused)
    plot("mine", mine % 360)
    wait(DT)
stop()

Run this in the simulator

Three lines: one smooth and slowly wrong, one correct and hairy, one that is smooth and stays correct.

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.

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?