Averaging and simple filters

Lesson 1 left us with a rule and a problem. The rule: averaging shrinks noise. The problem: averaging takes time, and a robot that waits to collect samples is a robot reacting to the past. This lesson explores that trade-off with the two simplest filters, the moving average and the exponential filter, and then shows the first real sensor-fusion idea, the complementary filter, which combines a gyroscope and an accelerometer to get a tilt angle better than either could give alone.

The moving average

A moving average replaces each reading with the mean of the last N readings:

y[k] = (x[k] + x[k-1] + ... + x[k-N+1]) / N

It is easy to understand and easy to code, and for independent noise it cuts the standard deviation by the square root of N. The cost is lag. When the true value jumps, the filter output only moves as new samples replace old ones in the window. On a steady ramp the output trails the truth by about (N - 1) / 2 samples. Larger windows mean cleaner output and slower response, and you cannot have both.

The exponential filter

The moving average needs a buffer of N values. The exponential filter gets a similar effect with a single stored number:

y[k] = alpha * x[k] + (1 - alpha) * y[k-1]

Each new output is a blend of the new reading and the previous output. The constant alpha between 0 and 1 is the trust you place in the newest reading. With alpha = 0.5 the filter listens closely to new data and smooths little. With alpha = 0.02 it barely reacts to any single reading and smooths a lot. Because older readings are multiplied by (1 - alpha) again and again, their influence decays exponentially, which is where the name comes from.

A useful rule of thumb: for white noise, an exponential filter with factor alpha smooths about as much as a moving average of N = 2 / alpha - 1 samples. So alpha = 0.1 is roughly a 19-sample window, but it needs one multiplication and no buffer, which is why it appears in nearly every microcontroller sketch.

Measure the trade-off

Let us test five filters on the same noisy signal: zero for 50 samples, then a step to 10, with noise of standard deviation 1. For each filter we measure two things. Noise is the standard deviation of the output once it has settled. Lag is how many samples the filter needs, after the step, to reach 9.0, which is 90 percent of the way. We measure lag on the clean step so noise does not trigger it early.

filter_tradeoff.py

ResetRun

import random import math random.seed(3) def ema(data, alpha): out, y = [], data[0] for x in data: y = alpha * x + (1 - alpha) * y out.append(y) return out def moving_average(data, window): return [sum(data[max(0, i - window + 1): i + 1]) / len(data[max(0, i - window + 1): i + 1]) for i in range(len(data))] clean = [0.0] * 50 + [10.0] * 350 # step at sample 50, 400 samples noisy = [c + random.gauss(0, 1) for c in clean] def spread(sig): # std dev after settling part = sig[300:] m = sum(part) / len(part) return math.sqrt(sum((v - m) ** 2 for v in part) / len(part)) def lag(sig): # samples to reach 90 percent return next(i - 50 for i in range(50, len(sig)) if sig[i] >= 9.0) filters = [ ("raw", lambda d: d), ("moving avg 5", lambda d: moving_average(d, 5)), ("moving avg 20", lambda d: moving_average(d, 20)), ("ema 0.5", lambda d: ema(d, 0.5)), ("ema 0.1", lambda d: ema(d, 0.1)), ("ema 0.02", lambda d: ema(d, 0.02)), ] print("filter noise sd lag to 90%") for name, f in filters: print("%-14s %6.2f %d" % (name, spread(f(noisy)), lag(f(clean))))

The raw signal shows a noise of about 1.0 and no lag. The 5-sample moving average drops the noise to about 0.47 and costs 4 samples of lag. The 20-sample window reaches about 0.12 but the output needs 17 samples to catch the step. Among the exponential filters, alpha = 0.5 gives about 0.59 with a lag of 3, alpha = 0.1 gives about 0.18 with a lag of 21, and alpha = 0.02 gives about 0.04 with a lag of 113 samples. Every row buys cleaner output with slower response, and the two columns move in opposite directions. (The noise figures for the slowest filters are rough, because their output is itself correlated from sample to sample and a 100-sample window is short.)

Fusing a gyro and an accelerometer

Now to the more interesting case: finding the tilt angle of a balancing robot. Two sensors can give it to you.

• The gyroscope measures angular rate. Integrating it gives the angle. It is smooth and responds instantly, but as we saw in lesson 1, its bias makes the integrated angle drift without limit.
• The accelerometer senses gravity. When the robot is not accelerating, the direction of gravity gives the tilt through an arctangent of two axes. It never drifts, but every bump and motor vibration shows up as noise.

The gyro is trustworthy over short times and wrong over long times. The accelerometer is the reverse. The complementary filter uses that fact directly:

angle = alpha * (angle + gyro * dt) + (1 - alpha) * accel_angle

Each step, we advance the previous angle by the gyro, then pull it gently toward the accelerometer's angle. In the short term the gyro dominates (weight alpha, typically 0.98). In the long term the small constant pull toward the accelerometer cancels the gyro drift. In frequency terms, it is a high-pass filter on the gyro and a low-pass filter on the accelerometer, which is why the weights add to one. The crossover happens at a time constant tau = alpha * dt / (1 - alpha); with alpha = 0.98 and dt = 0.01 s, that is 0.49 s. Disturbances faster than that come from the gyro, slower ones from the accelerometer.

Try it on a simulated robot

We swing the true angle as a 20-degree sine wave over 30 seconds. The gyro has a bias of 1 deg/s and a little noise. The accelerometer is unbiased but has a noise of 4 degrees, a bad vibration day. We compare the RMS error of three estimates.

complementary.py

ResetRun

import math import random random.seed(5) dt = 0.01 steps = 3000 # 30 seconds alpha = 0.98 angle_gyro = 0.0 angle_comp = 0.0 sq_acc = sq_gyro = sq_comp = 0.0 for i in range(steps): t = i * dt true_angle = 20 * math.sin(2 * math.pi * 0.1 * t) # deg true_rate = 20 * 2 * math.pi * 0.1 * math.cos(2 * math.pi * 0.1 * t) gyro = true_rate + 1.0 + random.gauss(0, 0.5) # bias 1 deg/s accel_angle = true_angle + random.gauss(0, 4.0) # noisy but unbiased angle_gyro += gyro * dt angle_comp = alpha * (angle_comp + gyro * dt) + (1 - alpha) * accel_angle sq_acc += (accel_angle - true_angle) ** 2 sq_gyro += (angle_gyro - true_angle) ** 2 sq_comp += (angle_comp - true_angle) ** 2 print("RMS error, accelerometer only: %.2f deg" % math.sqrt(sq_acc / steps)) print("RMS error, gyro only: %.2f deg" % math.sqrt(sq_gyro / steps)) print("RMS error, complementary: %.2f deg" % math.sqrt(sq_comp / steps))

The accelerometer alone has an error of about 4 degrees, exactly its noise level. The gyro alone reaches about 17 degrees, because 1 deg/s of bias accumulates into 30 degrees by the end of the run. The complementary filter achieves about 0.5 degrees, eight times better than the accelerometer and more than thirty times better than the gyro. Its small remaining error is mostly a constant offset equal to the bias multiplied by tau, about 1 * 0.49 deg: the filter cannot fully remove the gyro bias, it just keeps it bounded. A Kalman filter will do better by estimating the bias explicitly.