diff --git a/src/main/java/com/thealgorithms/streaming/KalmanFilter.java b/src/main/java/com/thealgorithms/streaming/KalmanFilter.java new file mode 100644 index 000000000000..5e6d5143b12d --- /dev/null +++ b/src/main/java/com/thealgorithms/streaming/KalmanFilter.java @@ -0,0 +1,287 @@ +package com.thealgorithms.streaming; + +/** + * A scalar (one dimensional) Kalman filter: the optimal way to track a quantity that drifts + * slowly while every measurement of it is noisy. + * + *
The filter carries two numbers: the current estimate {@code x} and how much it distrusts that + * estimate, the error variance {@code p}. Each step has two halves. + * + *
+ * predict: x <- x + u p <- p + q + * update: k <- p / (p + r) x <- x + k * (z - x) p <- (1 - k) * p + *+ * + *
where {@code q} is the process noise (how much the tracked quantity is expected to wander + * between two steps), {@code r} the measurement noise, {@code z} the measurement and {@code k} the + * Kalman gain. The gain is the whole story: it is the share of the measurement that gets believed. + * When the filter is unsure ({@code p} large) or the sensor is good ({@code r} small), {@code k} + * approaches 1 and the filter follows the sensor; in the opposite case it clings to its own + * prediction. Nothing tunes this by hand, the variances do it. + * + *
Fusing several sensors is not a separate algorithm: it is what happens when the same estimate + * is corrected once per sensor, each with its own noise level. A cheap sensor with a large {@code r} + * nudges the estimate a little, a precise one pulls it a lot, and the result is exactly the + * inverse-variance weighted combination that {@link #fuse(double[], double[])} computes in closed + * form: + * + *
{@code
+ * KalmanFilter filter = new KalmanFilter(startingHeight, 1.0, 0.01, 1.0);
+ * for (int t = 0; t < steps; t++) {
+ * filter.predict();
+ * filter.update(barometer[t], barometerVariance);
+ * filter.update(gps[t], gpsVariance); // second sensor, same estimate
+ * double height = filter.estimate();
+ * }
+ * }
+ *
+ * Both steps run in O(1) time and memory. This class is not thread-safe. + * + * @see Kalman filter + */ +public final class KalmanFilter { + + private final double processNoise; + private final double measurementNoise; + + private double estimate; + private double errorCovariance; + private double lastGain; + + private final double initialEstimate; + private final double initialErrorCovariance; + + /** + * Creates a filter. + * + * @param initialEstimate the starting guess for the tracked quantity + * @param initialErrorCovariance how uncertain that guess is; a large value makes the filter trust + * the first measurements almost completely + * @param processNoise variance added on every {@link #predict()}, i.e. how fast the quantity is + * expected to change on its own + * @param measurementNoise default variance of a measurement, used by {@link #update(double)} + * @throws IllegalArgumentException if any argument is not finite, or if a variance is negative, + * or if {@code measurementNoise} is zero + */ + public KalmanFilter(double initialEstimate, double initialErrorCovariance, double processNoise, double measurementNoise) { + requireFinite(initialEstimate, "initialEstimate"); + requireNonNegativeVariance(initialErrorCovariance, "initialErrorCovariance"); + requireNonNegativeVariance(processNoise, "processNoise"); + requirePositiveVariance(measurementNoise, "measurementNoise"); + + this.initialEstimate = initialEstimate; + this.initialErrorCovariance = initialErrorCovariance; + this.processNoise = processNoise; + this.measurementNoise = measurementNoise; + reset(); + } + + /** + * Advances the model by one step without any control input, growing the uncertainty by the + * process noise. + * + * @return the predicted estimate, unchanged in value for this constant model + */ + public double predict() { + return predict(0.0); + } + + /** + * Advances the model by one step, shifting the estimate by a known control input. + * + * @param controlInput the change the estimate is expected to undergo, e.g. velocity times the + * time step when tracking a position + * @return the predicted estimate + * @throws IllegalArgumentException if {@code controlInput} is not finite + */ + public double predict(double controlInput) { + requireFinite(controlInput, "controlInput"); + estimate += controlInput; + errorCovariance += processNoise; + return estimate; + } + + /** + * Corrects the estimate with a measurement taken by the default sensor. + * + * @param measurement the observed value + * @return the corrected estimate + * @throws IllegalArgumentException if {@code measurement} is not finite + */ + public double update(double measurement) { + return update(measurement, measurementNoise); + } + + /** + * Corrects the estimate with a measurement whose noise differs from the default one. Calling this + * several times per step, once per sensor, is the whole of sensor fusion. + * + * @param measurement the observed value + * @param noise variance of this particular measurement, strictly positive + * @return the corrected estimate + * @throws IllegalArgumentException if {@code measurement} is not finite or {@code noise} is not strictly positive + */ + public double update(double measurement, double noise) { + requireFinite(measurement, "measurement"); + requirePositiveVariance(noise, "noise"); + + double innovationVariance = errorCovariance + noise; + lastGain = errorCovariance / innovationVariance; + estimate += lastGain * (measurement - estimate); + // Algebraically this is (1 - gain) * p, but computing 1 - gain cancels away most of the + // significant digits whenever the gain is close to one, as it is on the first measurements. + errorCovariance = errorCovariance * noise / innovationVariance; + return estimate; + } + + /** + * Runs one full cycle: predict, then correct with the given measurement. + * + * @param measurement the observed value + * @return the filtered estimate + * @throws IllegalArgumentException if {@code measurement} is not finite + */ + public double filter(double measurement) { + predict(); + return update(measurement); + } + + /** + * Filters a whole signal offline, one cycle per sample. + * + * @param measurements the noisy signal + * @return a new array holding the filtered signal, of the same length + * @throws IllegalArgumentException if any measurement is not finite + * @throws NullPointerException if {@code measurements} is {@code null} + */ + public double[] filter(double[] measurements) { + double[] filtered = new double[measurements.length]; + for (int i = 0; i < measurements.length; i++) { + filtered[i] = filter(measurements[i]); + } + return filtered; + } + + /** + * Combines simultaneous readings of the same quantity taken by independent sensors, weighting + * each by the inverse of its variance. This is the closed form of what repeated + * {@link #update(double, double)} calls achieve within one step. + * + * @param measurements one reading per sensor + * @param variances the noise variance of each sensor, strictly positive, same length as {@code measurements} + * @return the fused reading together with its variance, which is never larger than the variance of + * the best single sensor + * @throws IllegalArgumentException if the arrays are empty, differ in length, hold a non-finite + * measurement or a non-positive variance + * @throws NullPointerException if either array is {@code null} + */ + public static Estimate fuse(double[] measurements, double[] variances) { + if (measurements.length != variances.length) { + throw new IllegalArgumentException("There must be exactly one variance per measurement, but got " + measurements.length + " and " + variances.length); + } + if (measurements.length == 0) { + throw new IllegalArgumentException("At least one measurement is required"); + } + + double weightSum = 0.0; + double weightedSum = 0.0; + for (int i = 0; i < measurements.length; i++) { + requireFinite(measurements[i], "measurement"); + requirePositiveVariance(variances[i], "variance"); + double weight = 1.0 / variances[i]; + weightSum += weight; + weightedSum += weight * measurements[i]; + } + return new Estimate(weightedSum / weightSum, 1.0 / weightSum); + } + + /** + * Returns the current estimate of the tracked quantity. + * + * @return the state estimate + */ + public double estimate() { + return estimate; + } + + /** + * Returns the variance of the current estimate; it shrinks with every update and grows with every + * prediction. + * + * @return the error covariance + */ + public double errorCovariance() { + return errorCovariance; + } + + /** + * Returns the Kalman gain used by the most recent update, a number in {@code [0, 1)} telling how + * much of that measurement was believed. + * + * @return the last gain, {@code 0} if no update has happened yet + */ + public double lastGain() { + return lastGain; + } + + /** + * Returns the process noise variance. + * + * @return the value given at construction time + */ + public double processNoise() { + return processNoise; + } + + /** + * Returns the default measurement noise variance. + * + * @return the value given at construction time + */ + public double measurementNoise() { + return measurementNoise; + } + + /** + * Restores the state the filter had right after construction. + */ + public void reset() { + estimate = initialEstimate; + errorCovariance = initialErrorCovariance; + lastGain = 0.0; + } + + @Override + public String toString() { + return "KalmanFilter{estimate=" + estimate + ", errorCovariance=" + errorCovariance + ", lastGain=" + lastGain + '}'; + } + + 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 requireNonNegativeVariance(double value, String name) { + if (!(value >= 0.0) || !Double.isFinite(value)) { + throw new IllegalArgumentException("The " + name + " must be finite and non-negative, but was " + value); + } + } + + private static void requirePositiveVariance(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); + } + } + + /** + * A value paired with the variance that describes how much it can be trusted. + * + * @param value the estimated quantity + * @param variance the variance of that estimate + */ + public record Estimate(double value, double variance) { + } +} diff --git a/src/test/java/com/thealgorithms/streaming/KalmanFilterTest.java b/src/test/java/com/thealgorithms/streaming/KalmanFilterTest.java new file mode 100644 index 000000000000..14037eb79476 --- /dev/null +++ b/src/test/java/com/thealgorithms/streaming/KalmanFilterTest.java @@ -0,0 +1,229 @@ +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 KalmanFilterTest { + + private static double rootMeanSquareError(double[] values, double truth) { + double sum = 0.0; + for (double value : values) { + sum += (value - truth) * (value - truth); + } + return Math.sqrt(sum / values.length); + } + + @ParameterizedTest + @ValueSource(doubles = {Double.NaN, Double.POSITIVE_INFINITY}) + void rejectsNonFiniteInitialEstimate(double initialEstimate) { + assertThrows(IllegalArgumentException.class, () -> new KalmanFilter(initialEstimate, 1.0, 0.1, 1.0)); + } + + @Test + void rejectsInvalidVariances() { + assertThrows(IllegalArgumentException.class, () -> new KalmanFilter(0.0, -1.0, 0.1, 1.0)); + assertThrows(IllegalArgumentException.class, () -> new KalmanFilter(0.0, 1.0, -0.1, 1.0)); + assertThrows(IllegalArgumentException.class, () -> new KalmanFilter(0.0, 1.0, 0.1, 0.0)); + assertThrows(IllegalArgumentException.class, () -> new KalmanFilter(0.0, 1.0, 0.1, Double.NaN)); + } + + @Test + void rejectsNonFiniteInputs() { + KalmanFilter filter = new KalmanFilter(0.0, 1.0, 0.1, 1.0); + assertThrows(IllegalArgumentException.class, () -> filter.predict(Double.NaN)); + assertThrows(IllegalArgumentException.class, () -> filter.update(Double.POSITIVE_INFINITY)); + assertThrows(IllegalArgumentException.class, () -> filter.update(1.0, 0.0)); + } + + @Test + @DisplayName("prediction adds uncertainty, correction removes it") + void covarianceGrowsOnPredictAndShrinksOnUpdate() { + KalmanFilter filter = new KalmanFilter(0.0, 1.0, 0.25, 1.0); + assertEquals(0.0, filter.lastGain()); + + filter.predict(); + assertEquals(1.25, filter.errorCovariance(), 1e-12); + + filter.update(2.0); + assertTrue(filter.errorCovariance() < 1.25); + assertTrue(filter.lastGain() > 0.0 && filter.lastGain() < 1.0); + } + + @Test + @DisplayName("the gain is the share of the measurement that gets believed") + void gainFollowsTheClosedForm() { + KalmanFilter filter = new KalmanFilter(0.0, 1.0, 0.0, 1.0); + filter.update(10.0); + assertEquals(0.5, filter.lastGain(), 1e-12); + assertEquals(5.0, filter.estimate(), 1e-12); + assertEquals(0.5, filter.errorCovariance(), 1e-12); + } + + @Test + void aControlInputShiftsTheEstimate() { + KalmanFilter filter = new KalmanFilter(100.0, 1.0, 0.1, 1.0); + assertEquals(103.0, filter.predict(3.0), 1e-12); + assertEquals(103.0, filter.estimate(), 1e-12); + } + + @Test + @DisplayName("with no process noise and no prior information the filter is the running mean") + void degeneratesIntoTheRunningMean() { + KalmanFilter filter = new KalmanFilter(0.0, 1e12, 0.0, 1.0); + Random random = new Random(2024L); + + double sum = 0.0; + for (int i = 1; i <= 200; i++) { + double measurement = random.nextGaussian(); + sum += measurement; + filter.update(measurement); + assertEquals(sum / i, filter.estimate(), 1e-6, "after " + i + " measurements"); + } + } + + @Test + @DisplayName("filtering a noisy constant beats using the raw measurements") + void reducesNoiseOnAConstantSignal() { + double truth = 12.5; + double noise = 2.0; + Random random = new Random(555L); + + double[] measurements = new double[400]; + for (int i = 0; i < measurements.length; i++) { + measurements[i] = truth + noise * random.nextGaussian(); + } + + KalmanFilter filter = new KalmanFilter(0.0, 100.0, 1e-4, noise * noise); + double[] filtered = filter.filter(measurements); + + assertEquals(measurements.length, filtered.length); + assertEquals(truth, filter.estimate(), 0.3); + assertTrue(rootMeanSquareError(filtered, truth) < rootMeanSquareError(measurements, truth) / 2.0); + } + + @Test + @DisplayName("a moving quantity is tracked, and more process noise means faster tracking") + void tracksAMovingSignal() { + Random random = new Random(31L); + double[] measurements = new double[200]; + for (int i = 0; i < measurements.length; i++) { + measurements[i] = i * 0.5 + random.nextGaussian(); + } + + KalmanFilter agile = new KalmanFilter(0.0, 1.0, 1.0, 1.0); + KalmanFilter sluggish = new KalmanFilter(0.0, 1.0, 1e-6, 1.0); + agile.filter(measurements); + sluggish.filter(measurements); + + double truth = 199 * 0.5; + assertTrue(Math.abs(agile.estimate() - truth) < Math.abs(sluggish.estimate() - truth), "agile=" + agile.estimate() + " sluggish=" + sluggish.estimate()); + assertEquals(truth, agile.estimate(), 3.0); + } + + @Test + @DisplayName("updating once per sensor reproduces the closed form inverse-variance fusion") + void sequentialUpdatesFuseSensors() { + double preciseReading = 10.0; + double preciseVariance = 0.25; + double coarseReading = 14.0; + double coarseVariance = 4.0; + + KalmanFilter filter = new KalmanFilter(0.0, 1e15, 0.0, 1.0); + filter.update(preciseReading, preciseVariance); + filter.update(coarseReading, coarseVariance); + + KalmanFilter.Estimate fused = KalmanFilter.fuse(new double[] {preciseReading, coarseReading}, new double[] {preciseVariance, coarseVariance}); + assertEquals(fused.value(), filter.estimate(), 1e-6); + assertEquals(fused.variance(), filter.errorCovariance(), 1e-6); + } + + @Test + void fusionWeightsSensorsByTheirPrecision() { + KalmanFilter.Estimate equal = KalmanFilter.fuse(new double[] {1.0, 3.0}, new double[] {1.0, 1.0}); + assertEquals(2.0, equal.value(), 1e-12); + assertEquals(0.5, equal.variance(), 1e-12); + + KalmanFilter.Estimate skewed = KalmanFilter.fuse(new double[] {1.0, 3.0}, new double[] {0.01, 1.0}); + assertTrue(skewed.value() < 1.1, "the precise sensor should dominate, but got " + skewed.value()); + assertTrue(skewed.variance() < 0.01, "fusing can only reduce the variance, but got " + skewed.variance()); + } + + @Test + void fusionOfASingleSensorReturnsIt() { + KalmanFilter.Estimate single = KalmanFilter.fuse(new double[] {7.0}, new double[] {2.0}); + assertEquals(7.0, single.value(), 1e-12); + assertEquals(2.0, single.variance(), 1e-12); + } + + @Test + void fusionRejectsMalformedInput() { + assertThrows(IllegalArgumentException.class, () -> KalmanFilter.fuse(new double[0], new double[0])); + assertThrows(IllegalArgumentException.class, () -> KalmanFilter.fuse(new double[] {1.0}, new double[] {1.0, 2.0})); + assertThrows(IllegalArgumentException.class, () -> KalmanFilter.fuse(new double[] {1.0}, new double[] {0.0})); + assertThrows(IllegalArgumentException.class, () -> KalmanFilter.fuse(new double[] {Double.NaN}, new double[] {1.0})); + } + + @Test + @DisplayName("fusing two sensors is better than trusting either of them alone") + void fusionBeatsEitherSensorAlone() { + double truth = 3.0; + Random random = new Random(97L); + double barometerNoise = 1.0; + double gpsNoise = 3.0; + + KalmanFilter filter = new KalmanFilter(0.0, 100.0, 1e-6, barometerNoise * barometerNoise); + double[] fused = new double[300]; + KalmanFilter barometerFilter = new KalmanFilter(0.0, 100.0, 1e-6, barometerNoise * barometerNoise); + + for (int i = 0; i < fused.length; i++) { + double barometer = truth + barometerNoise * random.nextGaussian(); + double gps = truth + gpsNoise * random.nextGaussian(); + + filter.predict(); + filter.update(barometer, barometerNoise * barometerNoise); + filter.update(gps, gpsNoise * gpsNoise); + fused[i] = filter.estimate(); + + barometerFilter.predict(); + barometerFilter.update(barometer); + } + + assertTrue(filter.errorCovariance() < barometerFilter.errorCovariance(), "fusing must not increase the uncertainty"); + assertEquals(truth, fused[fused.length - 1], 0.5); + } + + @Test + void resetRestoresTheInitialState() { + KalmanFilter filter = new KalmanFilter(5.0, 2.0, 0.1, 1.0); + assertEquals(0.1, filter.processNoise()); + assertEquals(1.0, filter.measurementNoise()); + + filter.filter(100.0); + filter.reset(); + + assertEquals(5.0, filter.estimate()); + assertEquals(2.0, filter.errorCovariance()); + assertEquals(0.0, filter.lastGain()); + } + + @Test + void toStringMentionsTheState() { + KalmanFilter filter = new KalmanFilter(1.0, 1.0, 0.1, 1.0); + assertTrue(filter.toString().contains("estimate=1.0"), filter.toString()); + } + + @Test + void rejectsNonFiniteVariances() { + assertThrows(IllegalArgumentException.class, () -> new KalmanFilter(0.0, Double.NaN, 0.1, 1.0)); + assertThrows(IllegalArgumentException.class, () -> new KalmanFilter(0.0, Double.POSITIVE_INFINITY, 0.1, 1.0)); + assertThrows(IllegalArgumentException.class, () -> new KalmanFilter(0.0, 1.0, Double.NaN, 1.0)); + assertThrows(IllegalArgumentException.class, () -> new KalmanFilter(0.0, 1.0, 0.1, Double.POSITIVE_INFINITY)); + } +}