Merge pull request #37 from StaticBeagle/alpha-beta-filter

Alpha beta filter

src/main/java/com/wildbitsfoundry/etk4j/signals/smoothing/AlphaBetaFilter.java

@@ -0,0 +1,53 @@
+package com.wildbitsfoundry.etk4j.signals.smoothing;
+// TODO tests and javadocs
+public class AlphaBetaFilter {
+    private double position; // Estimated position
+    private double velocity; // Estimated velocity
+    private final double alpha; // Smoothing factor for position
+    private final double beta; // Smoothing factor for velocity
+    private final double dt; // Time step (delta time)
+
+    // Constructor to initialize the filter
+    public AlphaBetaFilter(double initialPosition, double initialVelocity, double alpha, double beta, double dt) {
+        this.position = initialPosition;
+        this.velocity = initialVelocity;
+        this.alpha = alpha;
+        this.beta = beta;
+        this.dt = dt;
+    }
+
+    /**
+     * Update the filter with a new measurement.
+     *
+     * @param measuredPosition The observed position.
+     */
+    public void update(double measuredPosition) {
+        // Predict the next position
+        double predictedPosition = position + velocity * dt;
+
+        // Calculate the residual (error between measured and predicted positions)
+        double residual = measuredPosition - predictedPosition;
+
+        // Update position and velocity using alpha and beta gains
+        position = predictedPosition + alpha * residual;
+        velocity = velocity + beta * residual / dt;
+    }
+
+    /**
+     * Get the current estimated position.
+     *
+     * @return Estimated position.
+     */
+    public double getPosition() {
+        return position;
+    }
+
+    /**
+     * Get the current estimated velocity.
+     *
+     * @return Estimated velocity.
+     */
+    public double getVelocity() {
+        return velocity;
+    }
+}

src/test/java/com/wildbitsfoundry/etk4j/signals/smoothing/AlphaBetaFilterTest.java

@@ -0,0 +1,37 @@
+package com.wildbitsfoundry.etk4j.signals.smoothing;
+
+import org.junit.Test;
+
+import static org.junit.Assert.assertArrayEquals;
+
+public class AlphaBetaFilterTest {
+
+    @Test
+    public void testUpdate() {
+        // Example usage of the Alpha-Beta Filter
+        double initialPosition = 0.0;
+        double initialVelocity = 0.0;
+        double alpha = 0.85; // Position smoothing factor
+        double beta = 0.005; // Velocity smoothing factor
+        double dt = 1.0; // Time step (e.g., 1 second)
+
+        // Initialize the filter
+        AlphaBetaFilter filter = new AlphaBetaFilter(initialPosition, initialVelocity, alpha, beta, dt);
+
+        // Example measurements
+        double[] measurements = {0.0, 1.2, 2.8, 4.1, 5.9, 7.4, 9.2};
+
+        double[] expectedPosition = {0.0, 1.02, 2.5339, 3.8673154999999997, 5.5984912475, 7.1346751533875, 8.896429363503687};
+        double[] expectedVelocity = {0.0, 0.006, 0.01487, 0.022626149999999998, 0.03267644175, 0.04152060330375, 0.05163962452029375};
+        double[] actualPosition = new double[measurements.length];
+        double[] actualVelocity = new double[measurements.length];
+
+        for(int i = 0; i < measurements.length; i++) {
+            filter.update(measurements[i]);
+            actualPosition[i] = filter.getPosition();
+            actualVelocity[i] = filter.getVelocity();
+        }
+        assertArrayEquals(expectedPosition, actualPosition, 1e-12);
+        assertArrayEquals(expectedVelocity, actualVelocity, 1e-12);
+    }
+}