The classic pair is an accelerometer and a gyroscope measuring the same tilt. The accelerometer + * knows where down is and never drifts, but every vibration of the frame shows up in it. The + * gyroscope is smooth and immune to vibration, but it measures a rate, so using it means integrating, + * and the smallest bias in that rate integrates into an angle that walks away without limit. Neither + * is usable alone; their errors live in different parts of the spectrum, which is exactly the + * situation this filter is for. + * + *
+ * value <- a * (value + rate * dt) + (1 - a) * reference + *+ * + *
Read as a pair of filters that add up to one, it is a high pass on the integrated rate and a low + * pass on the absolute reading: the drift of the first is cut off below the corner frequency and the + * noise of the second above it. The two transfer functions sum to unity at every frequency, so the + * true signal passes through untouched whatever {@code a} is. That is where the name comes from, and + * it is also why the filter cannot introduce a lag of its own the way a plain low pass on the + * accelerometer would. + * + *
The single parameter is best thought of as a time constant rather than as a number near one: + * + *
+ * tau = a * dt / (1 - a) + *+ * + *
Below {@code tau} the answer comes from the gyroscope, above it from the accelerometer. That + * also fixes the price of the trade exactly: a gyroscope with a constant bias {@code b} leaves a + * steady state error of {@code tau * b} and no more, where plain integration would have grown without + * limit. Use {@link #ofTimeConstant(double, double)} to set it that way round. + * + *
Against {@link KalmanFilter}: the Kalman filter is the right answer when the noise of both + * sensors is known and worth modelling, and it will beat this one when it is. The complementary + * filter needs no covariance, no model of the process, two multiplications per sample and one number + * of state, and it degrades gracefully when the noise is not what anybody assumed. That is why it is + * what actually runs on small flight controllers. + * + *
{@code
+ * ComplementaryFilter tilt = ComplementaryFilter.ofTimeConstant(0.5, 0.01);
+ * for (Reading reading : imu) {
+ * double angle = tilt.accept(reading.gyroscopeRate(), reading.accelerometerAngle(), reading.dt());
+ * }
+ * }
+ *
+ * Each sample costs O(1) time and the filter keeps one number of state. This class is not + * thread-safe. + * + * @see KalmanFilter + * @see Complementary filter + */ +public final class ComplementaryFilter { + + /** Weight given to the integrated rate when none is chosen, the usual setting for an IMU. */ + public static final double DEFAULT_COEFFICIENT = 0.98; + + private final double coefficient; + + private double value; + private long count; + + /** + * Creates a filter that leans on the rate with the customary weight of {@code 0.98}. + */ + public ComplementaryFilter() { + this(DEFAULT_COEFFICIENT); + } + + /** + * Creates a filter. + * + * @param coefficient how much of the estimate comes from the integrated rate, in {@code (0, 1)}; + * closer to one trusts the rate for longer, closer to zero follows the reference more quickly + * @throws IllegalArgumentException if {@code coefficient} is outside {@code (0, 1)} + */ + public ComplementaryFilter(double coefficient) { + if (!(coefficient > 0.0) || !(coefficient < 1.0)) { + throw new IllegalArgumentException("The coefficient must lie in (0, 1), but was " + coefficient); + } + this.coefficient = coefficient; + } + + /** + * Creates a filter from the time constant that separates the two sensors, which is usually the + * quantity that is actually known: {@code a = tau / (tau + dt)}. + * + * @param timeConstant how long the rate is trusted before the reference takes over, strictly positive + * @param samplingInterval the interval between samples, strictly positive and in the same unit + * @return a new filter + * @throws IllegalArgumentException if either argument is not finite and strictly positive + */ + public static ComplementaryFilter ofTimeConstant(double timeConstant, double samplingInterval) { + requirePositive(timeConstant, "time constant"); + requirePositive(samplingInterval, "sampling interval"); + return new ComplementaryFilter(timeConstant / (timeConstant + samplingInterval)); + } + + /** + * Feeds one pair of readings taken one unit of time after the previous one. + * + * @param rate the reading of the drifting sensor, a derivative of the estimated quantity + * @param reference the reading of the noisy but drift free sensor, in the unit of the estimate + * @return the updated estimate + * @throws IllegalArgumentException if a reading is NaN or infinite + */ + public double accept(double rate, double reference) { + return accept(rate, reference, 1.0); + } + + /** + * Feeds one pair of readings. + * + * @param rate the reading of the drifting sensor, a derivative of the estimated quantity + * @param reference the reading of the noisy but drift free sensor, in the unit of the estimate + * @param elapsed time since the previous pair, strictly positive + * @return the updated estimate; the very first pair is answered with the reference alone, because + * there is nothing yet to integrate from + * @throws IllegalArgumentException if a reading is NaN or infinite, or if {@code elapsed} is not + * finite and strictly positive + */ + public double accept(double rate, double reference, double elapsed) { + requireFinite(rate, "rate"); + requireFinite(reference, "reference"); + requirePositive(elapsed, "elapsed time"); + + if (count == 0) { + value = reference; + } else { + value = coefficient * (value + rate * elapsed) + (1.0 - coefficient) * reference; + } + count++; + return value; + } + + /** + * Runs the filter over a whole pair of recordings sampled at unit intervals. + * + * @param rates the readings of the drifting sensor + * @param references the readings of the drift free sensor, as many as there are rates + * @return a new array of the same length holding the estimate after every sample + * @throws IllegalArgumentException if the two recordings differ in length or hold a reading that + * is NaN or infinite + * @throws NullPointerException if a recording is {@code null} + */ + public double[] scan(double[] rates, double[] references) { + if (rates.length != references.length) { + throw new IllegalArgumentException("Every rate needs a reference, but there were " + rates.length + " and " + references.length); + } + double[] estimates = new double[rates.length]; + for (int i = 0; i < rates.length; i++) { + estimates[i] = accept(rates[i], references[i]); + } + return estimates; + } + + /** + * Returns the current estimate. + * + * @return the estimate after the last pair of readings, {@code 0} before the first one + */ + public double value() { + return value; + } + + /** + * Returns the weight given to the integrated rate. + * + * @return the coefficient given at construction time + */ + public double coefficient() { + return coefficient; + } + + /** + * Returns the time constant the filter works out to at a given sampling interval, that is + * {@code a * dt / (1 - a)}: the horizon below which the rate decides the answer and above which + * the reference does. + * + * @param samplingInterval the interval between samples, strictly positive + * @return the time constant, in the unit of the interval + * @throws IllegalArgumentException if {@code samplingInterval} is not finite and strictly positive + */ + public double timeConstant(double samplingInterval) { + requirePositive(samplingInterval, "sampling interval"); + return coefficient * samplingInterval / (1.0 - coefficient); + } + + /** + * Returns how many pairs of readings have been filtered since the last reset. + * + * @return the sample count + */ + public long count() { + return count; + } + + /** + * Forgets everything seen so far, so that the next reference seeds the estimate again. + */ + public void reset() { + value = 0.0; + count = 0; + } + + /** + * Restarts the filter from a known estimate, which is what to do after the process has been moved + * by something the sensors could not see. + * + * @param estimate the value to carry on from + * @throws IllegalArgumentException if {@code estimate} is NaN or infinite + */ + public void reset(double estimate) { + requireFinite(estimate, "estimate"); + value = estimate; + count = 1; + } + + @Override + public String toString() { + return "ComplementaryFilter{coefficient=" + coefficient + ", value=" + value + ", samples=" + count + "}"; + } + + private static void requireFinite(double value, String name) { + if (!Double.isFinite(value)) { + throw new IllegalArgumentException("The " + name + " must be finite, but was " + value); + } + } + + private static void requirePositive(double value, String name) { + if (!(value > 0.0) || !Double.isFinite(value)) { + throw new IllegalArgumentException("The " + name + " must be finite and strictly positive, but was " + value); + } + } +} diff --git a/src/test/java/com/thealgorithms/streaming/ComplementaryFilterTest.java b/src/test/java/com/thealgorithms/streaming/ComplementaryFilterTest.java new file mode 100644 index 000000000000..0dc1db1a970d --- /dev/null +++ b/src/test/java/com/thealgorithms/streaming/ComplementaryFilterTest.java @@ -0,0 +1,258 @@ +package com.thealgorithms.streaming; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.Random; +import org.junit.jupiter.api.DisplayName; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.ValueSource; + +class ComplementaryFilterTest { + + private static final double COEFFICIENT = 0.98; + private static final double INTERVAL = 0.01; + + private static double standardDeviation(double[] values, double around) { + double sum = 0.0; + for (double value : values) { + sum += (value - around) * (value - around); + } + return Math.sqrt(sum / values.length); + } + + @ParameterizedTest + @ValueSource(doubles = {0.0, 1.0, -0.5, 1.5, Double.NaN, Double.POSITIVE_INFINITY}) + void rejectsACoefficientOutsideTheUnitInterval(double coefficient) { + assertThrows(IllegalArgumentException.class, () -> new ComplementaryFilter(coefficient)); + } + + @ParameterizedTest + @ValueSource(doubles = {0.0, -1.0, Double.NaN, Double.POSITIVE_INFINITY}) + void rejectsInvalidTimeConstants(double value) { + assertThrows(IllegalArgumentException.class, () -> ComplementaryFilter.ofTimeConstant(value, INTERVAL)); + assertThrows(IllegalArgumentException.class, () -> ComplementaryFilter.ofTimeConstant(0.5, value)); + assertThrows(IllegalArgumentException.class, () -> new ComplementaryFilter().timeConstant(value)); + } + + @ParameterizedTest + @ValueSource(doubles = {Double.NaN, Double.POSITIVE_INFINITY, Double.NEGATIVE_INFINITY}) + void rejectsNonFiniteReadings(double value) { + ComplementaryFilter filter = new ComplementaryFilter(); + + assertThrows(IllegalArgumentException.class, () -> filter.accept(value, 0.0)); + assertThrows(IllegalArgumentException.class, () -> filter.accept(0.0, value)); + assertThrows(IllegalArgumentException.class, () -> filter.reset(value)); + } + + @ParameterizedTest + @ValueSource(doubles = {0.0, -1.0, Double.NaN, Double.POSITIVE_INFINITY}) + void rejectsAnInvalidElapsedTime(double elapsed) { + ComplementaryFilter filter = new ComplementaryFilter(); + + assertThrows(IllegalArgumentException.class, () -> filter.accept(0.0, 0.0, elapsed)); + } + + @Test + void rejectsMismatchedRecordings() { + ComplementaryFilter filter = new ComplementaryFilter(); + + assertThrows(IllegalArgumentException.class, () -> filter.scan(new double[] {1.0, 2.0}, new double[] {1.0})); + } + + @Test + void exposesItsConfiguration() { + ComplementaryFilter filter = new ComplementaryFilter(0.9); + + assertEquals(0.9, filter.coefficient()); + assertEquals(0.98, new ComplementaryFilter().coefficient()); + assertEquals(0, filter.count()); + assertEquals(0.0, filter.value()); + } + + @Test + @DisplayName("the first pair is answered with the reference, because there is nothing to integrate yet") + void seedsWithTheFirstReference() { + ComplementaryFilter filter = new ComplementaryFilter(); + + assertEquals(7.5, filter.accept(100.0, 7.5), 1e-12); + assertEquals(1, filter.count()); + } + + @Test + @DisplayName("the coefficient and the time constant are two ways of saying the same thing") + void theTimeConstantRoundTrips() { + ComplementaryFilter filter = ComplementaryFilter.ofTimeConstant(0.5, INTERVAL); + + assertEquals(0.5, filter.timeConstant(INTERVAL), 1e-12); + assertEquals(0.5 / (0.5 + INTERVAL), filter.coefficient(), 1e-12); + assertEquals(COEFFICIENT * INTERVAL / (1 - COEFFICIENT), new ComplementaryFilter(COEFFICIENT).timeConstant(INTERVAL), 1e-12); + } + + @Test + @DisplayName("with nothing to integrate the estimate decays onto the reference geometrically") + void convergesOnTheReference() { + ComplementaryFilter filter = new ComplementaryFilter(COEFFICIENT); + filter.reset(10.0); + + for (int step = 1; step <= 200; step++) { + double value = filter.accept(0.0, 0.0); + assertEquals(10.0 * Math.pow(COEFFICIENT, step), value, 1e-9, "step " + step); + } + } + + @Test + @DisplayName("a biased rate leaves a bounded error of tau times the bias, where integration alone would run away") + void boundsTheDriftOfTheRate() { + double bias = 0.1; + ComplementaryFilter filter = new ComplementaryFilter(COEFFICIENT); + filter.reset(0.0); + + double integrated = 0.0; + for (int step = 0; step < 5000; step++) { + filter.accept(bias, 0.0, INTERVAL); + integrated += bias * INTERVAL; + } + + double expected = filter.timeConstant(INTERVAL) * bias; + assertEquals(expected, filter.value(), 1e-9, "the steady state error must be exactly tau * bias"); + assertEquals(0.049, filter.value(), 1e-6); + assertEquals(5.0, integrated, 1e-9, "plain integration of the same bias walks away"); + } + + @Test + @DisplayName("noise on the reference is cut down, roughly by the factor the theory promises") + void rejectsNoiseOnTheReference() { + Random random = new Random(7L); + ComplementaryFilter filter = new ComplementaryFilter(COEFFICIENT); + double[] references = new double[20000]; + double[] estimates = new double[references.length]; + + for (int i = 0; i < references.length; i++) { + references[i] = random.nextGaussian(); + estimates[i] = filter.accept(0.0, references[i], INTERVAL); + } + + double referenceSpread = standardDeviation(references, 0.0); + double estimateSpread = standardDeviation(estimates, 0.0); + double promised = Math.sqrt((1 - COEFFICIENT) / (1 + COEFFICIENT)); + + assertEquals(promised, estimateSpread / referenceSpread, 0.02, "the spread should shrink by sqrt((1-a)/(1+a))"); + } + + @Test + @DisplayName("a moving signal is followed without lag, which a low pass on the reference alone cannot do") + void followsARampWithoutLag() { + Random random = new Random(11L); + double slope = 1.0; + ComplementaryFilter filter = new ComplementaryFilter(COEFFICIENT); + + double lowPass = 0.0; + double filterError = 0.0; + double lowPassError = 0.0; + int steps = 4000; + + for (int i = 0; i < steps; i++) { + double truth = slope * i * INTERVAL; + double reference = truth + 0.05 * random.nextGaussian(); + double estimate = filter.accept(slope, reference, INTERVAL); + lowPass = i == 0 ? reference : COEFFICIENT * lowPass + (1 - COEFFICIENT) * reference; + + if (i > steps / 2) { + filterError += Math.abs(estimate - truth); + lowPassError += Math.abs(lowPass - truth); + } + } + + int counted = steps - steps / 2 - 1; + double filterMean = filterError / counted; + double lowPassMean = lowPassError / counted; + + assertTrue(filterMean < 0.02, "the complementary filter should sit on the ramp, it was off by " + filterMean); + assertEquals(filter.timeConstant(INTERVAL) * slope, lowPassMean, 0.02, "the low pass should lag by tau * slope"); + assertTrue(lowPassMean > 10 * filterMean, "the lag should dwarf the error of the complementary filter"); + } + + @Test + @DisplayName("the coefficient decides how quickly the reference takes over") + void theCoefficientDecidesWhichSensorWins() { + ComplementaryFilter trustsTheRate = new ComplementaryFilter(0.999); + ComplementaryFilter trustsTheReference = new ComplementaryFilter(0.001); + trustsTheRate.reset(0.0); + trustsTheReference.reset(0.0); + + for (int i = 0; i < 100; i++) { + trustsTheRate.accept(0.0, 100.0, INTERVAL); + trustsTheReference.accept(0.0, 100.0, INTERVAL); + } + + assertEquals(100.0 * (1 - Math.pow(0.999, 100)), trustsTheRate.value(), 1e-9); + assertTrue(trustsTheRate.value() < 10.0, "a second is a tenth of its time constant, so it has barely moved"); + assertEquals(100.0, trustsTheReference.value(), 1e-9, "the other one is on the reference from the first sample"); + assertTrue(trustsTheRate.timeConstant(INTERVAL) > 1000 * trustsTheReference.timeConstant(INTERVAL)); + } + + @Test + @DisplayName("the elapsed time scales how much of the rate is taken in") + void theElapsedTimeScalesTheIntegration() { + ComplementaryFilter fast = new ComplementaryFilter(COEFFICIENT); + ComplementaryFilter slow = new ComplementaryFilter(COEFFICIENT); + fast.reset(0.0); + slow.reset(0.0); + + fast.accept(2.0, 0.0, 0.5); + slow.accept(2.0, 0.0, 1.0); + + assertEquals(COEFFICIENT * 1.0, fast.value(), 1e-12); + assertEquals(COEFFICIENT * 2.0, slow.value(), 1e-12); + } + + @Test + void scanReportsOneEstimatePerSample() { + ComplementaryFilter filter = new ComplementaryFilter(); + + double[] estimates = filter.scan(new double[] {0.0, 0.0, 0.0}, new double[] {1.0, 1.0, 1.0}); + + assertEquals(3, estimates.length); + assertEquals(3, filter.count()); + assertEquals(1.0, estimates[2], 1e-12); + } + + @Test + void resetForgetsEverything() { + ComplementaryFilter filter = new ComplementaryFilter(); + filter.scan(new double[] {1.0, 1.0}, new double[] {5.0, 5.0}); + + filter.reset(); + + assertEquals(0, filter.count()); + assertEquals(0.0, filter.value()); + assertEquals(9.0, filter.accept(100.0, 9.0), 1e-12, "the next reference seeds the estimate again"); + } + + @Test + @DisplayName("restarting from a known estimate keeps it instead of waiting for the next reference") + void resetCanCarryOnFromAValue() { + ComplementaryFilter filter = new ComplementaryFilter(COEFFICIENT); + + filter.reset(4.0); + + assertEquals(4.0, filter.value(), 1e-12); + assertEquals(1, filter.count()); + assertEquals(COEFFICIENT * 4.0, filter.accept(0.0, 0.0), 1e-12); + } + + @Test + void toStringMentionsTheState() { + ComplementaryFilter filter = new ComplementaryFilter(0.9); + filter.accept(0.0, 2.0); + + String text = filter.toString(); + + assertTrue(text.contains("ComplementaryFilter")); + assertTrue(text.contains("coefficient=0.9")); + assertTrue(text.contains("samples=1")); + } +}