A drone's barometer gives its altitude to within a couple of centimetres, fifty times a second. That sounds precise, but the derivative term divides each change by dt = 0.02 s: a 2 cm wobble between two readings becomes a "vertical speed" of 1 m/s, and the derivative term passes that straight to the motors as buzzing.
The usual fix is to filter the derivative with a first-order low-pass filter (the first-order smoother of Level 2) of time constant Tf seconds:
raw = (y - y_previous) / dt (for the first reading, raw = 0)
D = D + alpha * (raw - D) with alpha = dt / (Tf + dt), and D = 0 before the first reading
D is the smoothed vertical speed (m/s). With Tf = 0, alpha = 1 and D is just the raw rate; a larger Tf averages over about the last Tf seconds, which removes the buzz but makes D slower to notice a real change in speed.
Write rate_filter(readings, dt, Tf) that returns the list of smoothed rates D, one per reading. Press Run with plot(raw_rates) and plot(rate_filter(readings, 0.02, 0.06)) on a noisy hover to see the buzz shrink, and on a steady climb to see the lag.
Examples
Input: readings = [10.0, 10.02, 9.99, 10.03, 10.0, 10.04], dt = 0.02, Tf = 0.06
Output: [0.0, 0.25, -0.1875, 0.359375, -0.10546875, 0.42089844]
Explanation: alpha = 0.02 / 0.08 = 0.25. The raw rates jump between -1.5 and +2 m/s;
the filtered ones stay within about ±0.4 m/s.
Input: readings = [0.0, 0.1, 0.2, 0.3, 0.4], dt = 0.1, Tf = 0.1
Output: [0.0, 0.5, 0.75, 0.875, 0.9375]
Explanation: a steady climb at 1 m/s; the filtered rate creeps up towards it.
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- See why differencing a noisy reading amplifies its noise
- Smooth the derivative with a first-order low-pass filter of time constant Tf
- Recognise the price of filtering: the smoothed rate lags behind the true one