Digital filters for robot sensors

Raw sensor data are rarely fit to use directly. Encoder velocity estimates are noisy, IMU readings carry vibration from motors and propellers, force sensors pick up structural ringing, and current measurements ripple at the PWM frequency. Digital filters clean these signals inside the controller, and choosing and implementing them well is one of the most practical skills in robotics. Every filter, however, is a trade: it removes some frequencies and inevitably delays or distorts others, and delay inside a feedback loop costs phase margin, as lessons 6, 7 and 9 showed. This lesson builds on the difference equations and z-transforms of lesson 10 to design the filters roboticists use every day: moving averages, first- and second-order IIR low-pass filters, high-pass filters, notch filters for motor and structural resonances, and differentiating filters for velocity. It compares FIR and IIR designs, explains group delay, and finishes with implementation advice for embedded code.

Specifying a filter

A filter is specified by what it should pass and what it should block. A low-pass filter passes frequencies below a cutoff and attenuates those above; a high-pass filter does the reverse; a band-pass filter passes a range; a band-stop or notch filter removes a narrow range. On a magnitude plot, the passband is where gain should stay near 1, the stopband is where gain must be small, and the transition band lies between. The steeper the transition, the higher the filter order needed and the more delay the filter adds. Specifications also cover phase: whether a constant delay is acceptable, and how much delay the control loop can tolerate. For control loops, delay is usually the binding constraint; for logged data processed offline, sharpness can be bought freely.

FIR filters: moving averages and beyond

An FIR filter computes a weighted sum of the last N inputs, \( y[n] = \sum_{k=0}^{N-1} h[k]x[n-k] \). The moving average, with all weights equal to 1/N, is the simplest. Its frequency response is

which has zeros at multiples of \( f_s/N \): a moving average over exactly one period of an interference frequency removes it completely. Averaging 20 samples at 1 kHz nulls 50 Hz and all its harmonics; averaging 50 samples at 3 kHz nulls 60 Hz and its harmonics. Its sidelobes are high, so it is a poor general low-pass filter, but it is excellent for white noise, which it reduces by \( \sqrt{N} \), and for removing a known periodic disturbance.

FIR filters with symmetric coefficients have linear phase: every frequency is delayed by the same amount, \( (N-1)/2 \) samples, so waveform shapes are preserved. They are always stable, because there is no feedback. Their drawback is length: a sharp FIR low-pass filter may need dozens or hundreds of taps, and its delay grows with its length. Windowed-sinc and least-squares methods design such filters; libraries such as SciPy provide them.

IIR low-pass filters

IIR filters achieve a given sharpness with far fewer coefficients, and far less delay, than FIR filters, by feeding back past outputs. The first-order low-pass filter,

is the workhorse of embedded robotics: one multiply-accumulate per sample, one state variable, and a cutoff \( f_c \) set by a single parameter. (The approximation \( \alpha = \tau/(\tau + T) \) used in ROB-220 gives nearly the same value when \( f_cT \) is small.) It rolls off at only −20 dB per decade, so it reduces noise well above its cutoff but does not separate close frequencies. Its delay at low frequency is about \( \tau = 1/(2\pi f_c) \).

When a steeper roll-off is needed, a second-order section, or biquad, provides −40 dB per decade:

Designing the biquad as a discretized Butterworth filter (maximally flat passband, \( \zeta = 0.707 \)) with the Tustin method gives a flat passband and no resonant peak. Higher-order filters are built as cascades of biquads, which is numerically far more robust than one high-order difference equation, especially in fixed-point or single-precision arithmetic where poles clustered near z = 1 (lesson 10) are sensitive to coefficient rounding. Bessel designs trade sharpness for nearly linear phase, minimizing overshoot on steps, which suits signals whose waveform shape matters.

High-pass filters and removing drift

A high-pass filter blocks DC and slow changes while passing faster ones. The first-order discrete high-pass filter, \( y[n] = \alpha(y[n-1] + x[n] - x[n-1]) \), removes slowly varying offsets such as thermal drift in a force sensor or the gravity component in an accelerometer when only vibration is of interest. The complementary filter of ROB-220 is a high-pass filter on the integrated gyroscope added to a low-pass filter on accelerometer tilt, with the two filters chosen so their sum is exactly one at every frequency.

Notch filters for resonances

Many robot problems involve a single troublesome frequency: a structural mode of an arm at 23 Hz, a belt resonance, propeller vibration at the blade-passing frequency, or mains hum. A notch filter removes a narrow band around a centre frequency \( \omega_0 \) while leaving others almost untouched. Its continuous form is

Zeros on the imaginary axis at \( \pm j\omega_0 \) give infinite attenuation exactly at \( \omega_0 \); the quality factor Q sets the width, with −3 dB points about \( \omega_0/Q \) apart. A narrow notch (high Q) disturbs other frequencies very little but must be tuned precisely; a wide notch tolerates frequency uncertainty but adds more phase lag below the notch, which can hurt a control loop whose crossover is nearby. Discretized with Tustin and prewarping, the notch becomes a single biquad. Servo drives commonly include several configurable notch filters, and some adapt their centre frequency automatically by tracking the vibration peak in an FFT.

Differentiating filters for velocity

Velocity is often estimated by differencing encoder positions, \( v[n] = (\theta[n] - \theta[n-1])/T \), whose transfer function \( (1 - z^{-1})/T \) has gain rising with frequency up to \( 2/T \) at the Nyquist frequency. Lesson 9 showed the result: quantization noise is amplified enormously. The standard remedy combines differentiation with low-pass filtering, \( v = \text{LPF}\{(1 - z^{-1})/T\} \), which is the discrete equivalent of the filtered derivative \( s/(\tau s + 1) \) used in the derivative term of PID controllers (ROB-300). Alternatives include measuring the time between encoder edges at low speed, fitting a line to the last few samples, and state observers or Kalman filters (ROB-310) that estimate velocity from a model.

import numpy as np
fs, N = 1000.0, 4000
T = 1/fs; t = np.arange(N)*T
true_v = 2.0*np.sin(2*np.pi*1.0*t)                   # rad/s
theta = np.cumsum(true_v)*T
q = 2*np.pi/4096
meas = np.round(theta/q)*q                            # 4096-count encoder
v_raw = np.diff(meas, prepend=meas[0])/T
def lpf(x, fc):
    a = np.exp(-2*np.pi*fc*T); y = np.zeros_like(x)
    for k in range(1, x.size): y[k] = a*y[k-1] + (1 - a)*x[k]
    return y
def rms(e): return np.sqrt(np.mean(e[500:]**2))
print(f"raw difference     : RMS error {rms(v_raw - true_v):.3f} rad/s")
for fc in [50, 20, 5]:
    v = lpf(v_raw, fc)
    lag_ms = 1000/(2*np.pi*fc)
    print(f"low-pass {fc:3d} Hz    : RMS error {rms(v - true_v):.3f} rad/s, delay about {lag_ms:5.1f} ms")

raw difference     : RMS error 0.606 rad/s
low-pass  50 Hz    : RMS error 0.128 rad/s, delay about   3.2 ms
low-pass  20 Hz    : RMS error 0.085 rad/s, delay about   8.0 ms
low-pass   5 Hz    : RMS error 0.273 rad/s, delay about  31.8 ms

Raw differencing gives large errors from quantization; low-pass filtering cuts them dramatically. But pushing the cutoff too low makes the error rise again, because the filter's delay makes the estimate lag the true velocity. The best cutoff balances noise against lag, a pattern that recurs in every estimation problem and that the Kalman filter of ROB-310 resolves optimally.

Group delay and the cost of filtering

The phase response \( \phi(\omega) \) of a filter determines how much each frequency is delayed. The group delay

is the delay experienced by a narrow-band signal near \( \omega \). A linear-phase FIR filter has constant group delay \( (N-1)T/2 \). IIR filters have frequency-dependent group delay, largest near the cutoff; a first-order low-pass filter delays low frequencies by about \( 1/(2\pi f_c) \). In a feedback loop, the filter's phase lag at the crossover frequency is what matters, and it should be budgeted along with sampling and computation delays.

For logged data, the delay can be eliminated altogether by zero-phase filtering: run the filter forward through the data, then backward. The phase lags cancel and the magnitude response is squared. Because it uses future samples, zero-phase filtering is non-causal and suitable only offline, for example when cleaning motion-capture data or analysing a test log.

Implementation details matter on embedded hardware. Use second-order sections for anything above second order; keep filter states in double or at least single precision floating point, or scale fixed-point arithmetic carefully; initialize states to the first measurement to avoid a start-up transient; and never filter inside a loop with a filter that saturates silently. Above all, filter only as much as needed: every hertz of cutoff removed costs delay.