Sensor Code and Filtering

Programming for Robotics 3 ended with a simulator in which the robot of Programming for Robotics 1 ran unchanged, and with two things the simulator made visible: a range reading that jitters by a few millimetres, and a wheel speed that flickers by one tick from pass to pass. Programming for Robotics 2 showed where such readings come from. This chapter is about what the program does with them. A sensor never delivers “the distance”. It delivers a number that is late, noisy, sometimes wild, sometimes missing and sometimes frozen, and the controller must be given something better. The chapter builds that something in code: a Sensor class that says whether a reading can be believed, a set of small filters, code that measures a noise record and chooses a filter from it, fusion of two sensors, line and encoder code, calibration, and checks that notice a dead sensor. It uses the formulas of Robotics Fundamentals 5 section 9 (noise and filtering), section 4 (line position) and section 10 (calibration), and of Robotics Fundamentals 6 section 2 (speed from counts), each tied to the code that implements it. Every filter is tested on the simulator, whose noise is at first the easy kind (Gaussian, no outliers); the chapter then breaks the sensor on purpose to show what the easy case hides.

1. From a reading to a trustworthy number

Between the hardware and the decision there are four jobs, and a robot that does them in the right order, each in its own small piece of code, is much easier to debug than one that mixes them:

• Validate. Is it a number at all, and inside what the part can really measure? None, NaN and out-of-range values are not measurements. Say so once, in one place (section 2).
• Filter. Reduce the noise and remove the wild values, at a known price in delay (sections 3 to 5).
• Combine and convert. Fuse two sensors, turn raw counts into metres, a row of line sensors into a position (sections 6 to 9).
• Watch. Notice a sensor that has stopped telling the truth, and turn that into the safe default of Programming for Robotics 1 section 11 (section 10).

The order matters. A filter placed before validation averages a NaN into every later output. A health check placed after a heavy filter sees a calm signal and cannot tell it from a frozen one. And every filter costs delay, so the least filtering that does the job is the right amount.

2. The Sensor class: a value, a flag, a timestamp

The pattern is a thin wrapper around whatever function reads the hardware. It holds the newest valid value, whether the latest read was valid, the time of the newest valid value, and from that its age and whether it is stale. It never raises because of the sensor: the bus error of Programming for Robotics 2 section 11 becomes an invalid reading, not a crash in the middle of the controller. Two policies for a number outside the sensor’s range are offered, because they mean different things. By default it is invalid (the part is lying: a 4.35 m reading from a sensor that stops at 4 m is not 4.35 m). With clamp=True it is limited to the range and flagged: useful when the physical meaning is “at least this far” or “at least this near”, as for a reading that is just under the minimum range.

The class has no filtering and no thresholds of its own; it only answers four questions: what is the value, is it valid, how old is it, is it too old. The first block below feeds it a scripted list of raw readings, a plain one and a clamping one side by side, with a clock that moves 0.02 s per reading and a timeout of 0.05 s:

Read the table row by row. A None, a NaN and a bus error (OSError) are all invalid and give None; the age grows by 0.02 s with each of them, and the value becomes stale once the age passes the 0.05 s timeout (the 4th and 5th rows). The reading 0.012 is below the 0.02 m minimum: the plain sensor rejects it, the clamping sensor returns 0.020 and sets clamped. A later valid reading resets the age to zero and the run of bad readings to zero. The counters (bad, bad_run) are the raw material of section 10.

The range sensor against a real simulated world

The simulator’s range sensor answers None in two different situations (Programming for Robotics 3 section 6): nothing within the maximum range, and a reading that noise pushed outside the limits. A real HC-SR04 style sensor times out in the first case and may do the second. The code cannot tell which it was, and the choice of what to do is the robot’s: treat the first as “far” if the program knows the arena is smaller than the range, treat it as a failure otherwise. The block below uses two small test tools (place, which teleports the robot along x, a step in the world that a real robot could not make, and collect, which takes a list of readings) and a 6 m long arena, with the robot facing the far wall, so that some readings are beyond the 4 m limit:

At x = 0.3 m and 1.0 m the wall is 5.63 m and 4.93 m away: nothing in range, None. At 2.5 m and 5.8 m the sensor reads close to the truth (3.4268 against 3.4300, and 0.1267 against 0.1300: the noise, a few millimetres). At 5.90 m the true range is 0.03 m and the reading is None: the noise (the standard deviation is 5 mm) pushed it below 0.02 m, which the sensor reports as no measurement. This is the near end of the range, and it is why a minimum range matters: a robot that stops at a range of 0.05 m may be using readings that are partly None. Notice also the age at the end: after a None the sensor still holds the last valid value, and it is 0.04 s old; the stale flag (a 0.11 s timeout here, as in Programming for Robotics 1 section 6) is what turns a run of missing readings into a stop.

3. Filters as small stateful classes

A filter is a function with a memory. Written as a class, the memory is explicit, the filter can be reset, copied, tested and logged, and a pipeline of filters is just a list. All the filters below share the interface of the Filter base class: update(x) takes one reading and returns the filtered value, a bad input (None, NaN or infinity) is ignored and the previous output is held (so one bad value cannot poison the state), and reset() forgets everything. The two first filters are the ones of Robotics Fundamentals 5 section 9.

The moving average, in constant time

The mean of the last N readings is y_k = (x_k + x_k−1 + … + x_k−N+1) / N. Computing that sum afresh each time costs N additions per update. But the sum changes by only two terms when a new reading arrives, so the update is y_k = y_k−1 + (x_k − x_k−N) / N: add the new, subtract the one that falls out. The oldest reading is kept in a ring buffer: a list of N slots and an index that goes round and round, so the slot about to be overwritten always holds the oldest value. Each update touches one slot and the running sum, in time that does not depend on N:

While the buffer is still filling, the unused slots hold 0.0 and the count of slots in use is divided by instead of N, so the first outputs are the mean of what has arrived. One honest warning: a running sum accumulates rounding errors, slowly. resync() recomputes the sum from the buffer; the block of section 3 shows how small the drift is.

The median: one wild value never gets through

The median of N readings (N odd) is the middle one when they are sorted. A single wild reading is the largest or the smallest of the window, never the middle, so it is ignored completely, and a sharp step in the true value is passed on without being smeared. The price is a sort of N values per update (cheap for N of 3 to 9) and less smoothing of ordinary noise than an average. With N = 2m + 1 the median survives up to m wild values in any window.

The exponential filter, and its time constant

y_k = y_k−1 + α (x_k − y_k−1), with 0 < α ≤ 1: each new reading moves the output a fraction α of the way towards it. One stored number, one multiplication. It is the digital version of the RC low-pass filter. After a step of size S in the input the output has covered the fraction 1 − (1 − α)ⁿ of it after n samples, which crosses 63.2 % at about n = 1 / (−ln(1 − α)) samples, roughly (1 − α) / α. With a sample interval dt that is the time constant τ = dt (1 − α) / α, and solving for α gives the form to use when the time constant is what you know: α = dt / (τ + dt). A worked example: dt = 0.02 s and a wanted τ of 0.1 s give α = 0.02 / 0.12 = 0.1667; check: 0.02 × 0.8333 / 0.1667 = 0.100 s (the exact time constant, −dt / ln(1 − α) = 0.02 / 0.1823 = 0.1097 s, is 10 % larger: the rule of thumb is close but not exact when α is not small). The class can be built from either. It starts at the first reading, not at zero: starting at zero would drag every later output up from nothing.

Robotics Fundamentals 5’s outlier table, from the classes

Robotics Fundamentals 5 section 9 compared the three filters on twelve readings of a wall 100 cm away, with one wild echo (130.0). The classes reproduce its table digit for digit, which is the first check that the code implements the formulas it is supposed to:

Look at the outlier at k = 5. The moving average of 3 jumps to 109.90 and stays wrong for three outputs, because the 130.0 is inside its window of 3. The exponential filter jumps by 7.5 and decays, each step keeping 75 % of the error. The median of 3 is never more than 0.3 off: a single wild value never becomes the middle one. The last line shows the drift of the running sum: after 200000 updates on numbers near 1000 the running sum of the 8-point average differs from a fresh sum by 3 × 10⁻⁸ (a relative error of about 10⁻¹¹), and resync() brings it to zero. For a robot that runs for hours at 50 Hz that is harmless, and a resync every few thousand updates is one line if you want certainty.

Three small non-linear pieces: slew limiter, Schmitt trigger, debouncer

Three more filters are not about noise in a measured quantity but about what the program does with it. They are short, and each fixes a failure that the averaging filters do not:

• Slew-rate limiter. The output follows its target but moves at most max_rate units per second. On a command it is a ramp (a smooth start: the ramping of motor commands in Programming for Robotics 5, planned; not yet written). On a measurement it is a spike guard: a robot cannot move faster than its top speed, so a range that jumps by metres in one pass is not believable, and the limiter lets it creep towards the new value instead of taking the jump. Its weakness: a real jump is followed slowly too.
• Hysteresis (a Schmitt trigger). A yes/no decision from a noisy number, such as “obstacle near”, flips back and forth when the number hovers around the threshold. Two thresholds fix it: turn on at 0.25 m, turn off again only above 0.28 m. The gap between them should be a few times the noise (here 30 mm against a noise of 5 mm, six sigma).
• Debouncer. A switch or a bumper contact bounces: it flickers on and off for a few passes before it settles. The debouncer changes its output only after N readings in a row disagree with it. The cost is N passes of delay on every real change; unlike a median it is for yes/no signals.

Last, a Pipeline chains filters: the output of each is the input of the next. The usual one is a median first (it removes the wild values, which would otherwise be smeared by the next stage) and an average or exponential filter second (it smooths what is left).

All four on synthetic signals, where the right answer is known:

The slew limiter turns a jump of 0.3 into a ramp of 0.012 per pass (0.6 m/s per second times 0.02 s): 0.060 after 5 passes, 0.180 after 15, the full 0.300 after 25. As a spike guard it turns a 1.8 m jump into 0.02 m (1.00 to 1.02), and the next pass returns to 1.00. The bare comparison of the noisy approach with the 0.25 m threshold changes its answer 9 times as the robot creeps from 0.30 m to 0.20 m, and the Schmitt trigger exactly once. The debouncer swallows the two single-pass blips at the start (0010110111) and follows the real press, 4 passes late (the raw sequence turns to 1 for good at pass 8, the debounced one at pass 11, hence 0000000000111…), then ignores the brief dips and releases only when the zeros really stay.

4. What filtering costs: delay

Every smoothing filter answers with a past, not a present: its output is the average of what the sensor said over the last few samples, so a changing quantity shows up late. The cost is a number you can compute, and the simulator can measure it, with the noise turned off so that the delay is seen alone.

The delay of a moving average. The N-point average gives each of the last N readings the weight 1/N. Their ages are 0, 1, …, N − 1 samples, whose mean is (N − 1) / 2. That mean age is the delay: for a signal changing at a steady rate, the output equals what the input was (N − 1) / 2 samples ago. With N = 5 it is 2 samples, 0.04 s at 50 Hz.

The delay of an exponential filter. The weight on a reading that is n samples old is α (1 − α)ⁿ, and the mean age is the sum of n α (1 − α)ⁿ, which is (1 − α) / α: the same number as τ / dt, the time constant in samples. For α = 0.25 it is 3 samples; for α = 1/3, 2 samples.

A useful consequence. Setting (1 − α) / α = (N − 1) / 2 gives α = 2 / (N + 1): the exponential filter that has the same delay as an N-point average. And since the exponential filter reduces the variance of independent noise by the factor α / (2 − α), this α gives (2 / (N + 1)) / (2 N / (N + 1)) = 1 / N, exactly the average’s. To first order the two are equivalent, and the exponential filter needs one number of memory instead of N. (The average forgets a reading completely after N samples; the exponential filter forgets it gradually and never quite completely, which matters when an outlier gets in.)

The function below is the delay of a filter on a step: step_response returns the fraction of a step in the range that the output has covered at each sample after the step, and the delay is the sum of the missing fractions, 1 − s_k (the area between the step and the response; for a moving average s_k = (k + 1) / N for k < N, and the sum of 1 − (k + 1) / N is (N − 1) / 2). A small block of formulas for the delay, the noise ratios and the failure probability of a median comes first, so that the measurements can be compared with them:

Every measured delay equals the formula to two decimals: 2.00 samples for the 5-point average, 4.00 for the 9-point one, 3.00 for the exponential filter with α = 0.25 and 2.00 for α = 1/3, and the median of 5 also has a delay of 2 samples on a step (the output switches after 3 of the 5 samples in the window are new). The pipeline of a median of 5 and an exponential filter of 0.5 has 2 + (1 − 0.5) / 0.5 = 3 samples: the delays of the stages add. The last column is a different measure, the number of samples until the output has covered 90 % of the step: 5 for the average of 5, 3 for the median, 9 for the exponential filter with α = 0.25. The average is quicker to finish than the exponential filter of the same delay (it reaches 100 % after N samples, the other only approaches it), which is the price of the exponential filter’s long tail.

The shape of the response, for the two filters with the same delay of 2 samples (the average of 5 and α = 1/3) and a unit step:

The moving average climbs in a straight line over exactly 5 samples and stops; the exponential filter jumps at once to a third of the step and then approaches the final value ever more slowly. Both have the area of 2 samples between them and the step, so both lag a ramp by 2 samples.

Delay costs distance

A filter delay of D samples means that the number the controller sees is D samples old. For a robot approaching a wall at speed v, the range it reads is too large by D × v × dt. The block below drives the simulated robot at the wall at a steady 0.3 m/s (duty 0.75), lets the motors settle and the filters fill for 1.5 s, and averages the error of the filtered range against the true range of the same sensor over the next 2.5 s:

The prediction and the measurement agree within 0.2 mm for every filter: a delay of 2 samples is 12 mm at 0.3 m/s, 4 samples are 24 mm, and the pipeline of 3 samples costs 18 mm. The extra 0.2 mm is the mean of the noise over the 125 samples (5 mm / √125 = 0.45 mm of standard error). 12 mm does not sound much, but it adds to the stop line: a robot that stops at a filtered range of 0.25 m has really stopped 12 mm short of it while it is still moving, and the delay of the motors adds the 2.4 cm of Programming for Robotics 3 section 5. When a controller needs the speed of a decision, count the delays of the whole chain, as in the latency chain of Programming for Robotics 1 section 12.

5. Choosing a filter from a measured noise record

The right filter is not a matter of taste. Three numbers decide it, all of which can be read off a record of the sensor looking at something that does not move: the noise (standard deviation of the ordinary scatter), the outlier rate (how often a reading is wild), and the delay the controller can afford. The record is taken like a measurement of a real sensor: park the robot facing a wall, collect a few thousand readings, and compare each with the true distance (which a real experiment gets from a tape measure).

Four statistics do the work, all from the standard library. The mean and the (sample) variance, s² = sum (x − mean)² / (n − 1), are the ordinary ones, but a single wild reading ruins both. So there is also a robust sigma, from the median absolute deviation (MAD): 1.4826 times the median of |x − median|. For Gaussian noise it equals the standard deviation, and wild values barely move it, because the median ignores them. An outlier is then a reading further than 4 robust sigmas from the median. And an ascii_histogram draws the shape of the scatter in the terminal:

The robot is parked 0.9300 m from the wall (the sensor is 7 cm ahead of the centre). The 4000 readings have a mean error of +0.095 mm (no bias: the simulated sensor is perfect on average), a standard deviation of 4.995 mm (the 5 mm that was put in), and a robust sigma of 5.235 mm. The robust one is an estimate with a scatter of about 2 % from 4000 readings (about 0.1 mm), so this one is two standard errors high: close enough, but not exactly the same number as the plain one. The outlier rate is 0.0000: no reading is more than 4 robust sigmas away, as expected from a Gaussian. The histogram is the bell curve of Programming for Robotics 3 section 6: 754 readings in each of the two middle bins, about two thirds within ±5 mm, almost none beyond 15 mm.

The noise falls as one over the square root of N, and what that assumes

Robotics Fundamentals 5 section 9 gave the standard error of a mean: σ / √N for N independent readings. For an exponential filter the factor is √(α / (2 − α)). Both can be checked on the same record:

The measured spread is within 1 to 6 % of the prediction for every filter, a little above it because the output of a filter is estimated from fewer independent values than the 4000 readings (a window of 25 has only 160 independent windows in 4000 readings, so its estimate is good to about 5 %). The exponential filter with α = 0.2 behaves like the 9-point average (α = 2 / (9 + 1) = 0.2), and α = 2/17 like the 16-point one, as section 4 said. The gain is slow: the 25-point average cuts the noise to a fifth (1.06 mm) at a delay of 12 samples, a quarter of a second at 50 Hz.

The word independent hides a trap. The formula holds only if each reading is a new, independent measurement. A real range sensor measures every 25 ms or so, and a control loop that runs faster than the sensor reads the same value several times. The simulator can do the same (period_s, Programming for Robotics 3 section 6): here the sensor makes a new measurement every 0.1 s, so every value is read five times in a row by the 50 Hz loop:

An average of 5 fed on every pass reduces the noise by only 0.83 (instead of 0.45): its window mostly holds copies of one measurement. Fed only the new measurements, it reaches 0.45 = 1 / √5. The lesson for the code: update a filter when there is a new reading, not when the loop runs. A real driver should return a “new data” flag or a timestamp, and the Sensor class could keep one (here, as in the demonstration, a changed value stands in for it, which is not safe on a real sensor that can repeat a value by chance).

Outliers: breaking the sensor on purpose

The simulator’s noise is Gaussian and has no outliers, which is the easy case. Real ultrasonic sensors have a second kind of error: an echo off the wrong object, a missed pulse, a spike from the motors, a reading of the right shape and the wrong size. Everything that matters about a filter shows up there. So the next piece of code breaks the sensor on purpose: FaultyHardware wraps the simulator, which is a hardware object (Programming for Robotics 1 section 7), and replaces a chosen fraction of the range readings by a wrong echo, uniform between 0.05 m and 3.0 m. It passes everything else (ticks, set_duty, now_s, advance) through unchanged. This is fault injection, and it needs no new simulator code. The same wrapper can freeze the sensor at a chosen time, which section 10 uses.

The assumed rate is 10 %, higher than a good sensor in a quiet room shows, and the right order of magnitude for a cheap ultrasonic sensor among furniture (an assumption: measure yours):

The mean error jumps to +58.8 mm and the plain standard deviation to 317 mm: both are ruined by the 395 wild readings. The robust sigma moves from 5.2 to 5.87 mm, a little, because 10 % of far-away values shifts the median of the deviations a few percent up the bell curve. The outlier rate beyond 4 robust sigmas is 0.098: the injected 10 %, less the 1.4 % of the wrong echoes that happen to land within a few millimetres of the truth (the spike range is 2.95 m wide and the ±20 mm of the truth is 0.04 m of it). A plain standard deviation on this record would have said the sensor is 60 times noisier than it is. That is why the robust numbers belong in the measuring code: they describe the part that a filter can smooth, and the outlier rate describes the part it must reject.

Which filter?

The block below scores six candidates on the same record, as the root-mean-square error against the true range, the largest error, and the number of outputs more than 20 mm off:

The averages and the exponential filters are worse than useless here: the 5-point average has an rms error of 153 mm, half the raw 322 mm, because each wild echo is spread over five outputs (1482 outputs are off by more than 20 mm, against 389 raw readings: the filter has made the problem 4 times as wide). The median of 5 removes it: an rms of 33 mm and only 23 bad outputs. Chaining the exponential filter after it smooths the 5 mm of ordinary noise: an rms of 23 mm and a noise floor around the 3 mm that the formula predicts (the median has already done part of the smoothing), at a delay of 3 samples. The 60 bad outputs, against 23 for the median alone, are the median’s own failures (a majority of wild values in a window) spread over several outputs by the exponential filter.

The last line is a check with the binomial formula. A median of N fails when a majority of its window is wild: with a wild-value probability p, the chance that at least (N + 1) / 2 of N are wild is the sum, over k from (N + 1) / 2 to N, of C(N, k) p^k (1 − p)^N−k. For N = 5 and p = 0.10 that is 10 × 0.001 × 0.81 + 5 × 0.0001 × 0.9 + 0.00001 = 0.0086 (0.0082 when p is the 0.0986 left after the spikes that land near the truth). So there should be up to 33 bad outputs in 3970, and 23 were measured. The formula is an upper bound: when the wild values fall on both sides of the truth, the middle value of the window can still be a good one. Use the same formula the other way round to choose N for a given outlier rate: for p = 1 % a median of 3 fails with probability 3 × 10⁻⁴, a median of 5 with 1 × 10⁻⁵.

The rules that come out of this:

• Outliers present? Median first. Pick N from the outlier rate with the binomial formula and from the delay you can afford (the delay of a median is (N − 1) / 2 samples). A moving average or an exponential filter before the median, or alone, smears every wild value over several outputs.
• Ordinary noise too large? Then smooth, with the price in mind. Pick N or α from the noise ratio you need (1 / √N, or √(α / (2 − α))) and check the delay it costs: (N − 1) / 2 or (1 − α) / α samples, times v dt metres of lag at the speed you drive.
• No outliers, little noise? Do not filter. Delay is the price; the controller of Programming for Robotics 6 (planned; not yet written) is the one that will pay it. A quick “obstacle ahead?” decision needs a median of 3 or 5 and a Schmitt trigger, not a 25-point average.
• Measure again on the real sensor. The simulator’s numbers are assumptions (Programming for Robotics 3 section 13); the noise record of a real sensor, taken in the room it will work in, is the input to this section.

6. Fusing two range sensors

Two sensors that measure the same thing, with different noise, can give an estimate better than either. The best way to combine two independent estimates x₁ and x₂ with variances v₁ = σ₁² and v₂ = σ₂² is the inverse-variance weighted mean. Take the combination x = w₁ x₁ + w₂ x₂ with w₁ + w₂ = 1 (so that a true value of both gives a true value). Its variance is w₁² v₁ + w₂² v₂, which is smallest when each weight is proportional to the inverse of its variance:

Two things follow from the formula. The fused variance 1 / (1/v₁ + 1/v₂) is always smaller than the smaller of the two, so adding even a poor sensor never hurts if the weights are right. And the weights need the variances, so the noise record of section 5 is not a luxury: it is where v₁ and v₂ come from. The function takes any number of (value, variance) pairs and skips a missing value, so it keeps working when one sensor drops out:

The block below uses the simulator with two range sensors at the same mount: the front sensor with a standard deviation of 5 mm and a second one (add_range_sensor) with 2 mm. The worked example is repeated first as code, then 4000 readings of both are fused:

The by-hand example gives 0.9294 m and a standard deviation of 1.857 mm, as in the worked example above. On the 4000 readings the fused error has a standard deviation of 1.85 mm, against 1.86 mm predicted, better than the 1.97 mm of the better sensor alone, while the plain average is worse than it (2.72 against 2.69 predicted). The method has three limits. The errors must be independent: two sensors that share a bias (the speed of sound for two ultrasonic sensors, a common power-supply ripple) do not average it away. A bias in one is carried into the result, scaled by its weight, so calibrate first (section 8). And the variances must be right: a sensor believed to be better than it is will dominate, and the fusion will follow it wherever it goes. The Kalman filter of Programming for Robotics 10 (planned; not yet written) is this same weighting, repeated over time, with a model of how the quantity moves.

7. Line sensor code

A reflectance array gives one number per sensor, and the program wants one number for the robot: where the line is. Robotics Fundamentals 5 section 4 gave the steps; here they are code, tested against its worked example and then against the simulator’s painted line. The simulator’s own line sensor (Programming for Robotics 3 section 6) answers 0 or 1 per sensor, which is the “on or off only” case. To have analogue values, with the awkward properties of real ones (each sensor with its own floor and line level, noise, partial coverage), the test rig AnalogLine samples the paint at seven points across each sensor’s 10 mm light spot and turns the covered fraction into ADC counts between that sensor’s white and black level, plus noise. It is a rig, not part of a robot: on the real robot its raw() is replaced by the ADC reads of Programming for Robotics 2 section 2.

The code of the chapter proper has three parts. LineCalibration is the min/max calibration routine: while the robot sweeps over the floor and the line, keep each sensor’s lowest and highest raw value, and turn any raw value into a weight w_i = (raw_i − lo_i) / (hi_i − lo_i), limited to 0..1 (Robotics Fundamentals 5 section 4). line_position is the weighted average x = sum(w_i x_i) / sum(w_i), with sensor positions x_i = (i − (n − 1) / 2) × pitch, positive to the right, a noise floor under which a weight counts as zero, and the line declared lost when sum(w) is below 0.2. LineTracker adds a plan for the lost line: for a short time hold the last position, then report lost and remember which side the line was last seen on, which is the direction to search:

Calibration first. A 19 mm stripe is painted across the robot’s path, and the robot drives over it at 0.2 m/s (a sweep over both floor and line, as the real routine does by hand or by spinning in place):

Each sensor found its own floor and line level, a few counts beyond the true ones: the lowest reading of 100 noisy readings is the true floor level minus a couple of standard deviations of noise (8 counts), and the highest the line level plus a couple. That is the weakness of min/max calibration: a single noise spike sets the extreme, and with a larger noise the weights would be a few percent compressed. The calibration check ok() matters more: a sensor whose span is small never saw both the floor and the line (the sweep missed the line, or the sensor is dead), and the robot should refuse to start and not follow garbage. The two weights at the end show what normalisation does: the same raw reading 1000 is 0.216 of the way to black on sensor 0 and 0.198 on sensor 2, because their levels differ.

Now the formula. The worked example of Robotics Fundamentals 5 section 4 (8 sensors, pitch 8 mm) as code:

The code reproduces both numbers of the book, 4.69 mm and 5.33 mm. The first is the position with the weights 0.30, 1.00 and 0.45 at x = −4, +4 and +12 mm: (0.30 × (−4) + 1.00 × 4 + 0.45 × 12) / 1.75 = 8.20 / 1.75. The second shows what a single noisy white-floor reading of 0.05 on a sensor far from the line (x = +28 mm) does: it moves the answer by 0.64 mm, because it has a long lever arm. A noise floor of 0.10 ignores it, and the answer is 4.69 mm again. Reading on or off only gives 4.0 mm, a step of 8 mm at a time.

A second small worked example, for the 5-sensor array of the simulator (pitch 15 mm) and the 19 mm line, with ideal coverage: a line centred 7.5 mm to the right covers 70 % of the light spot of sensor 2 (the centre) and 70 % of sensor 3 (at +15 mm), and nothing else. The weighted average is (0.7 × 0 + 0.7 × 15) / 1.4 = 7.5 mm, exactly. Finally the whole thing against the simulator, where the true position is known. The robot starts 12 mm to the left of a line (so the line is 12 mm to its right), heading very slightly towards it, and drives along the line for 9 s; the line ends at x = 2.0 m:

The measured position follows the true one to within 0.4 mm in six of the seven rows shown (9.0 and 9.0, then 6.7 against 6.6, and so on); at t = 5.0 s the error is 1.4 mm, close to the largest of the whole run. Over the 388 passes on the line the error has a root-mean-square of 0.50 mm and a largest value of 1.45 mm. Using only the on/off decisions at the same threshold, the root-mean-square error is 2.95 mm and the largest 7.27 mm: the weighted average is six times as precise with the same hardware, as Robotics Fundamentals 5 promised. When the line ends the position is held for 0.1 s and then declared lost, at t = 7.86 s, when the sensors are 26 mm beyond the end of the line (the paint has a rounded end, 9.5 mm beyond its last point, plus the 0.1 s hold, 20 mm at this speed, less up to one pass of 4 mm). side() says “left” because the line was last at −7 mm: for a robot that has drifted off the line, that is the direction to turn to find it again. Here the line really ended, which is why a line follower also needs a rule for that (a stop, or a search): behaviours are the subject of Programming for Robotics 7 (planned; not yet written).

8. Calibration code

A calibration removes the systematic part of a sensor’s error by comparing it with known true values. Robotics Fundamentals 5 section 10 gave the formulas; the class below implements both. The model is a straight line, reading = gain × true + offset, and the inverse true = (reading − offset) / gain is what the program applies. Two points make the line pass through both exactly: gain = (r₂ − r₁) / (t₂ − t₁), offset = r₁ − gain t₁. Least squares chooses the line through all the points that minimises the sum of squared errors: with S_xx = sum (t − mean t)² and S_xy = sum (t − mean t)(r − mean r), gain = S_xy / S_xx and offset = mean r − gain × mean t. The true values are on the x axis because they are the ones you know; the noise is in the readings. residuals gives, for each point, the calibrated value minus the true value: the evidence of whether the straight line was a good model.

The numbers of Robotics Fundamentals 5 section 10 (a distance sensor against true distances of 10 to 100 cm), with the worked least-squares arithmetic: the means are 53 and 55.46, S_xx = 43² + 23² + 3² + 22² + 47² = 5080, S_xy = 5107.6, so gain = 5107.6 / 5080 = 1.00543 and offset = 55.46 − 1.00543 × 53 = 2.172:

The class reproduces the book’s numbers: a two-point calibration with true = 0.99448 r − 2.331 (gain 1.00556 in the other direction), a reading of 77.3 cm is really 74.54 cm, its errors at the three check points are −0.41, −0.02 and −0.46 cm; the least-squares line reading = 1.00543 true + 2.172 has errors +0.17, −0.23, +0.16, −0.28 and +0.18 cm, the largest of them 0.28 cm, smaller than the 0.46 cm of the two-point line, as the book said. Two practical points from the book matter in the code: average many readings at each calibration point (a two-point calibration puts the noise of two readings straight into the gain and the offset), and keep a check point that is not used in the fit.

A nonlinear sensor: calibrate in a linear variable

A straight line is the wrong model for an infrared triangulation sensor, whose output is strongly nonlinear in distance (Robotics Fundamentals 5 section 3: V is about k / d, so the spot moves as 1 / d). The sensor of the rig below, IRSensor, behaves like that: V = k / (d + d0) with k = 0.27 V m and d0 = 0.02 m, a noise of 10 mV, read by a 12-bit ADC with a 3.3 V reference (an assumed sensor, of the form of the common ones; I have not measured one). The remedy is to change variable: 1 / V = (d + d0) / k is a straight line in d, with gain 1/k and offset d0/k. So the same LinearCalibration fits it, with the readings replaced by 1 / counts, and the inverse, d = (1/counts − offset) / gain, turns a reading into a distance: the “inverse-distance linearisation fitted in code”.

The block places the robot at eight known distances from the wall, takes 50 readings at each, fits a straight line of counts against distance (a) and a straight line of 1 / counts against distance (b), and then checks both at five distances that were not used in the fit:

The counts fall from 2793 at 0.10 m to 411 at 0.80 m, a factor of seven, and not linearly. The fit in the inverse variable recovers the sensor’s own constants: d = 335.9 / counts − 0.0210 m, against the true k = 0.27 V m = 335.0 counts m and d0 = 0.0200 m, which is 0.3 % and 1 mm off. Its errors at the five check distances are −0.5, −0.1, +0.5, +0.7 and +3.1 mm. The straight line through the counts, fitted by least squares through the same points, is wrong by 67 to 136 mm, which is as bad as having no calibration: the model was wrong, and no amount of data would have helped (the residuals of a good fit have no pattern; here they would have been large and systematic, which is what residuals is for). The last line shows the price: the noise of one calibrated reading is 1.2 mm at 0.15 m and 18.6 mm at 0.70 m, since 1/d is steep near the sensor and flat far away, so a fixed ADC noise means a large distance noise far out. That is the reason to filter an infrared sensor harder at long range, and to trust it less. Two caveats from memory of the sensor type rather than the simulation: many real infrared sensors give a lower voltage at very short range (below a few centimetres), which makes the reading ambiguous, and their output depends on the colour and the surface of the target.

9. Encoders: ticks, speed and wrap-around

Programming for Robotics 2 section 9 decoded the quadrature signals into one signed tick count per wheel, and Programming for Robotics 1 section 6 turned ticks into metres (0.340 mm per tick for the build robot’s 600 ticks per revolution and 65 mm wheels). This section does what is left: a speed that is steadier than one tick per pass, and a count that survives wrapping around.

Counters wrap around

A hardware counter register has a fixed number of bits and wraps from its maximum back to zero (or the other way). An 8-bit counter wraps every 256 ticks, 8.7 cm of travel for this robot; a 16-bit counter every 65536 ticks, 22.3 m. A program that subtracts the previous count from the new one gets a huge jump at each wrap. The fix is a modular difference: the change of an N-bit counter is the difference reduced modulo 2^N into the range −2^N−1 .. +2^N−1 − 1. In code, ((new − old + 2^N−1) mod 2^N) − 2^N−1. Worked example, 8 bits: the counter goes from 250 to 5; the plain difference is −245; the modular one is ((5 − 250 + 128) mod 256) − 128 = (−117 mod 256) − 128 = 139 − 128 = +11: it went forward 6 ticks to 255, wrapped, and another 5. The method assumes the true change between two readings is less than 2^N−1: it cannot tell a change of +200 from one of −56. That is a limit on the loop period: the counter must be read at least every 2^N−1 × (metres per tick) / v_max seconds. For 8 bits at 0.4 m/s that is 128 × 0.340 mm / 0.4 = 0.11 s.

SpeedEstimator unwraps the counts into a growing total, and then does two things to make the speed steadier. It divides the change over the last window passes by the real time those passes took (the timestamps, not the nominal dt, so jitter in the loop does not become noise in the speed), and it can follow that with an exponential filter. WrappingTicks is a rig: it wraps the simulator’s tick count at a chosen number of bits, as a real register would.

The robot drove 0.384 m (1128 ticks). The 8-bit counter now reads 104. Plain subtraction accumulates 104 ticks, 0.035 m: it only knows that the counter went from 0 to 104, and has lost the four wraps. (The sum of plain differences is the last count minus the first, whatever happened in between.) The modular difference counts 1128 ticks, 0.3839 m, which is the truth to within the 0.00034 m of a single tick (the position is the exact one, the count is rounded down to whole ticks). The three delta_ticks examples are the worked example in the text. And the last line is the loop-period limit: at 0.4 m/s an 8-bit counter has to be read every 0.11 s. On a real robot the wrap is rarely an 8-bit counter, but a 16-bit timer counter in encoder mode at a loop that stalls for a second is exactly this problem. The same arithmetic in C++ happens for free with integer types of the right width, which is worth knowing because it is the usual way the problem is solved on an Arduino (from memory; not compiled here):

Speed from ticks: quantisation and smoothing

The speed from the change in ticks over an interval dt is v = Δticks × (metres per tick) / dt (Robotics Fundamentals 6 section 2). The ticks are whole numbers, so the speed moves in steps of one tick: the resolution is metres per tick / dt, which is 0.00034034 / 0.02 = 0.0170 m/s here, independent of how fast the robot goes. In revolutions per second it is 1 / (CPR × dt) = 1 / (600 × 0.02) = 0.0833 rev/s. At 0.3 m/s the robot moves 17.63 ticks per pass, so the count is 17 or 18, and the speed is 0.289 or 0.306 m/s, as Programming for Robotics 3 section 8 showed. The spread of that flicker can be predicted. If f is the fractional part of the ticks per pass (here 0.63), the count is the larger value a fraction f of the time, so the standard deviation of the speed is the resolution × √(f (1 − f)), at most half a resolution step (f = 1/2). Over a window of N passes the resolution improves to (1 / N) of that, with the f of N passes of travel:

The prediction for one pass, 0.0082 m/s, matches the measured spread of the raw speed, 0.0083 m/s, and the 5-pass window predicts 0.0012 m/s and measures 0.0012. The mean is exactly right in all four rows (0.3000 m/s): quantisation does not bias a speed over time, it only flickers. So the choice is the usual trade of noise for delay: the 5-pass window cuts the spread by 7 times, at the cost of a slower response, 0.24 s to reach 90 % of the speed, against 0.18 s for the true speed (the motor’s own lag, the 0.08 s time constant of Programming for Robotics 3 section 5: ln 10 × 0.08 = 0.184 s) and 0.18 s for the raw estimate. The exponential filter with α = 0.2 is almost as smooth (0.0011) and slower (0.32 s), and the combination of a 5-pass window and α = 0.5 has the smallest spread, 0.0006 m/s, at 0.26 s. Which is right depends on the loop that uses the speed: a speed controller (Programming for Robotics 6, planned; not yet written) is the customer that pays for delay, so it takes the shortest window that its resolution allows. For very slow speeds, timing the gap between ticks is better than counting them (Robotics Fundamentals 6 section 2), but it needs a timestamp for each tick from the interrupt handler of Programming for Robotics 2 section 9, which a polling loop does not have.

10. Sensor health: stuck, bad and stale, and the safe default

The Sensor class gives a value that is valid or not. It does not notice a sensor that is valid and wrong: one that has frozen on a reading that is perfectly in range. A frozen range is the most dangerous failure of this chapter, because everything downstream looks healthy: the filter is calm, the controller is happy, and the robot drives at the wall at full speed while its sensor insists the wall is 1.7 m away. The health monitor looks for the three signs, each with a number you choose:

• Stuck. The same value, exactly, stuck_n valid readings in a row. A real sensor with noise almost never repeats a value: if the chance of a repeat is p per reading, a run of n has chance pⁿ, which for even p = 0.5 and n = 20 is one in a million. A sensor with a coarse ADC and a very quiet signal repeats often, so stuck_n must be large for those; and clamped readings (the range limit) are excluded because they repeat legitimately. The cost of the check is detection time: stuck_n passes (0.4 s at 50 Hz and 20), during which the robot moves 12 cm at 0.3 m/s.
• Bad. bad_max invalid readings in a row (None, NaN, out of range). One is a dropped echo; ten in a row is a sensor that has stopped working.
• Stale. No valid reading for the sensor’s timeout (the 0.11 s of Programming for Robotics 1 section 6).

Health watches a Sensor and names the fault. FilteredHardware then puts the whole pipeline of this chapter behind the same three methods as the hardware: range_m() returns the validated, filtered value, and returns None when the health check has a fault. The Robot of Programming for Robotics 1 needs no change at all: its own timeout logic (Programming for Robotics 1 section 6 and 11) counts the age of a missing range and stops with the fault “range stale”, which is exactly the safe default of that chapter, fed by a better detector. A stuck sensor’s readings are kept out of the filter (so the filter does not keep averaging a frozen number) and a good sensor’s readings resume the output when the fault clears, since a new valid value resets the run of bad ones.

Two experiments, in both the Robot of Programming for Robotics 1 is used unchanged, with the simulator for a world and FaultyHardware for the fault. In the first the sensor has 10 % of wrong echoes, and the robot drives at the wall to the stop line of 0.25 m. Because FilteredHardware already filters, the robot’s own exponential filter is switched off for the third case with a configuration that has RANGE_FILTER_ALPHA = 1.0. The score is the root-mean-square difference between the speed the robot commands and the speed it should command for the true range:

With a clean sensor the robot’s command is within 2 mm/s (an rms of 0.0021 m/s) of the ideal, it stops 0.250 m from the wall, and nothing is bumped. With 10 % of wrong echoes and only its own exponential filter, the rms command error is 0.079 m/s, the command is off by the full 0.300 m/s at the worst moment (for example a wild reading of 0.1 m makes the robot brake, and one of 2 m makes it speed up near the wall), and 12 passes end in a collision with the wall and the robot ends 0.020 m from it, touching. With a median of 5 and the exponential filter behind FilteredHardware, the same sensor gives an rms error of 0.014 m/s, no bumps, and a stop 0.236 m from the wall, 14 mm inside the stop line: an occasional wild reading that survives the median makes the robot surge forward (the same pipeline on a clean sensor stops at 0.2497 m). The largest error is still 0.300 m/s: a median of 5 fails when three of five readings are wild, about once in 120 windows at this rate, and then the robot is wrong for a pass or two. A filter reduces the damage of a bad sensor; it does not make the sensor good.

In the second experiment the sensor freezes at t = 3.0 s (stuck_after_s), repeating its last range of about 1.7 m for ever, while the robot keeps driving:

Without health checks the robot sees a calm, believable 1.7 m, never raises a fault, drives at 0.30 m/s into the wall and pushes against it: 160 of the 600 passes end in a collision, and it stops at x = 2.910 m only because the wall stops it. With the checks, the health monitor notices at about 3.4 s that the value has not changed for 20 readings (the stuck run), the range goes missing, the robot’s own timeout turns it into the fault “range stale” at t = 3.48 s (x = 1.314 m, still 1.5 m from the wall), and the wheel command is 0.00 m/s from then on. The robot stopped for the right reason (the health check says “stuck”, the robot says “range stale”) with no bump. Log both: “range stale” alone would hide whether the sensor was silent or frozen, and the two have different causes.

11. Testing filters, with synthetic signals and with the simulator

A filter looks right on a plot and fails on the first strange input, so each filter gets tests, and the best tests need no robot: synthetic signals whose correct output you can work out by hand. Four signals cover most of the ground:

• A constant. The output must equal the input (to rounding), whatever the filter. It catches a wrong normalisation (a moving average divided by the wrong number) and a wrong start-up.
• A step. The output must follow a response you can write down: a 5-point average goes 0.2, 0.4, 0.6, 0.8, 1.0; a median goes from 0 to 1 at the third new sample; an exponential filter with α = 0.25 goes 0.25, 0.4375, 0.578125.
• A spike. One wild value: a median must ignore it completely, an average must spread exactly spike / N over exactly N outputs and then forget it.
• Garbage. None, NaN, infinity: the output must not change, and the next good reading must give the same answer as if the garbage had never arrived. This is the test that finds the NaN that poisons a running sum for ever.

Add one property test with random input (a fixed seed): the output of a median or a moving average always lies between the smallest and the largest of the last N inputs. It catches off-by-one errors in the ring buffer that no hand-made case thought of. The tests below are plain functions that use assert, and a ten-line runner finds every function called test_… in a namespace and runs it (the same job pytest does, with no installation):

Then the tests that need the simulator, written the same way: the noise of a 9-point average must be a third of the sensor’s, the delays measured on a step must match the formulas, and a run at the wall with a dirty sensor behind FilteredHardware must never bump. They are defined in the block below, which then runs everything in the session:

Twenty-three tests pass. Two of them (test_stops_before_the_wall and test_stops_when_sensor_goes_silent) are not from this chapter: they are the tests of Programming for Robotics 1 section 7, which are in the same session, and the runner found them because it looks for the name, not the place. If the module is run as a file, the last two lines (below) run the tests that do not need the simulator. A failing test names itself. Two bugs were put into the module on purpose, to see: dividing by self.n instead of self.count in the moving average is caught by test_filters_stay_inside_the_data and by no other test (the others all start with a full buffer; the start-up averaged in empty slots), and forgetting to subtract the oldest reading fails four tests, the constant, the step, the spike and the property test. A test that nothing else duplicates, like the first, is the one that earns the random-input test its place. Keep these tests: the next change to a filter will be made in a hurry.

12. The reference listing: filters.py

The blocks of this chapter that have no output and are not marked as written from memory, in the order of the page, are the whole module. Save it as filters.py. It imports only math, random and collections, it needs neither robot.py nor sim.py (the rigs AnalogLine, IRSensor, WrappingTicks and FaultyHardware only use methods that a simulator or a robot has), and running it as a file runs its own tests. The demonstration blocks stay in the chapter; later chapters import what they need from here: from filters import Sensor, MedianFilter, ExpFilter, Pipeline, FilteredHardware.

13. Challenges

Work these with Python, the simulator and the module of this chapter. The solutions are text files with the working, and the code in them was run, so the output shown is what it printed.

Next: Programming for Robotics 5, Driving: Motors and Odometry

This chapter made the robot’s senses trustworthy: readings that say whether they are valid, filters whose delay is known, a speed that does not flicker by one tick, a calibration, and a monitor that stops the robot when a sensor lies. Programming for Robotics 5 (planned; not yet written) turns to the other half of the loop, the robot’s body. It writes the motor driver code (ramping with the slew limiter of this chapter, speed limits, the dead zone), uses the smoothed wheel speeds of section 9 to know what each wheel is really doing, and updates the robot’s pose from the encoder counts, the odometry whose drift Programming for Robotics 3 measured against the formula. It then drives straight lines and turns by an angle with it, and calibrates the wheel diameter and the track width from measured runs, so that the robot goes where it is told. The hardware notes for the build robot’s motors are there too.