feat: add alpha beta filter (#171)

* add alpha beta filter

* fix unit test

* Apply suggestions from code review

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

---------

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

Author: gabrielfrasantos

README.md

@@ -21,7 +21,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal
 | [Controllers](doc/controllers/README.md)                           | Bang-Bang/Hysteresis, PID, LQR, MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF |
 | [Dynamics](doc/dynamics/README.md)                                 | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler, ABA            |
 | [Estimators](doc/estimators/README.md)                             | Linear Regression, Yule-Walker (offline), Recursive Least Squares (online) |
-| [Filters](doc/filters/README.md)                                   | Kalman, Extended Kalman, Unscented Kalman, Complementary, FIR, IIR, Exponential Moving Average, Moving Average, Median Filter |
+| [Filters](doc/filters/README.md)                                   | Kalman, Extended Kalman, Unscented Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter |
 | [Kinematics](doc/kinematics/README.md)                             | Forward Kinematics                                                   |
 | [Neural Network](doc/neural_network/README.md)                     | Layers, activations, losses, model                                   |
 | [Optimization](doc/optimization/README.md)                         | Gradient Descent                                                     |

ROADMAP.md

@@ -27,7 +27,6 @@ Difficulty legend:
 
 | #  | Component                                            | Target module             | Difficulty |
 |----|------------------------------------------------------|---------------------------|------------|
-| 8  | Alpha-beta / alpha-beta-gamma filter                 | `filters/active`          | ★★☆☆☆      |
 | 10 | Gain-scheduled controller                            | `controllers`             | ★★☆☆☆      |
 | 11 | Convolution & correlation utilities                  | `analysis`                | ★★☆☆☆      |
 | 12 | Polynomial least-squares curve fitting               | `estimators/offline`      | ★★☆☆☆      |

doc/filters/active/AlphaBetaFilter.md

@@ -0,0 +1,103 @@
+# Alpha-Beta / Alpha-Beta-Gamma Filter
+
+## Overview & Motivation
+
+In embedded control and tracking applications, a sensor delivers a position measurement every sample period, but that measurement is corrupted by noise. A simple lowpass filter smooths the noise but cannot estimate velocity, which is needed for prediction and control. A full Kalman filter computes optimal gains but requires covariance propagation — a matrix inverse every step — which is too expensive for a fast ISR.
+
+The alpha-beta (and its extension, alpha-beta-gamma) filter resolves this tension. It maintains a position and velocity estimate (and optionally acceleration) using only a few multiply-adds per sample. The gains are fixed constants, computed once at design time from a single scalar parameter. The result is a deterministic, constant-time predictor-corrector that delivers most of the benefit of a steady-state Kalman filter at a fraction of the cost.
+
+## Mathematical Theory
+
+### State Model
+
+The filter assumes constant-velocity (order 2) or constant-acceleration (order 3) kinematics. For order 2, the state is $\mathbf{x} = [p, \dot{p}]^\top$; for order 3, $\mathbf{x} = [p, \dot{p}, \ddot{p}]^\top$.
+
+### Predict Step
+
+$$\hat{p}^- = \hat{p} + T_s \hat{v} + \tfrac{1}{2} T_s^2 \hat{a} \quad (\hat{a} \text{ omitted for order 2})$$
+$$\hat{v}^- = \hat{v} + T_s \hat{a} \quad (\hat{a} \text{ omitted for order 2})$$
+$$\hat{a}^- = \hat{a}$$
+
+### Correct Step
+
+Let the innovation (residual) be $r = z - \hat{p}^-$, where $z$ is the measured position. Then:
+
+$$\hat{p} = \hat{p}^- + \alpha r$$
+$$\hat{v} = \hat{v}^- + \frac{\beta}{T_s} r$$
+$$\hat{a} = \hat{a}^- + \frac{2\gamma}{T_s^2} r \quad (\text{order 3 only})$$
+
+The denominators $T_s$ and $T_s^2$ convert the dimensionless residual into velocity and acceleration corrections.
+
+### Kalata Steady-State Design (Tracking Index)
+
+For the order-2 case, Kalata (1984) defines the tracking index $\lambda = \frac{\sigma_w T_s^2}{\sigma_v}$, where $\sigma_w$ is process noise intensity and $\sigma_v$ is measurement noise standard deviation. The critically-damped gains are:
+
+$$r = \frac{4 + \lambda - \sqrt{8\lambda + \lambda^2}}{4}$$
+$$\alpha = 1 - r^2$$
+$$\beta = 2(2 - \alpha) - 4\sqrt{1 - \alpha}$$
+
+A single scalar $\lambda$ thus controls the smoothing/lag trade-off.
+
+### Stability Conditions
+
+For the order-2 filter, Simpson's triangle requires:
+
+$$0 < \alpha < 1, \qquad 0 < \beta < 4 - 2\alpha$$
+
+Violation of the second bound causes oscillatory divergence.
+
+## Complexity Analysis
+
+| Case    | Time   | Space  | Notes                                           |
+|---------|--------|--------|-------------------------------------------------|
+| Best    | $O(1)$ | $O(N)$ | $N \in \{2, 3\}$ state words plus fixed scalars |
+| Average | $O(1)$ | $O(N)$ | same                                            |
+| Worst   | $O(1)$ | $O(N)$ | gains are precomputed; no covariance update     |
+
+The hot path is a handful of fused multiply-add operations: predict costs 2–4 MACs, correct costs 2–3 MACs.
+
+## Step-by-Step Walkthrough
+
+Consider an order-2 filter with $\alpha = 0.5$, $\beta = 0.1$, $T_s = 1.0\,\text{s}$, measuring a ramp $z[n] = 0.2n$.
+
+| Step     | $z$ | $\hat{p}^-$ | $\hat{v}^-$ | $r$  | $\hat{p}$ | $\hat{v}$ |
+|----------|-----|-------------|-------------|------|-----------|-----------|
+| 0 (seed) | 0.0 | —           | —           | —    | 0.0       | 0.0       |
+| 1        | 0.2 | 0.0         | 0.0         | 0.2  | 0.10      | 0.020     |
+| 2        | 0.4 | 0.12        | 0.020       | 0.28 | 0.26      | 0.048     |
+| …        | …   | …           | …           | …    | …         | …         |
+
+After several hundred steps, $\hat{v} \to 0.2$ and lag $\to 0$.
+
+## Pitfalls & Edge Cases
+
+- **Small $T_s$**: the corrections $\beta/T_s$ and $2\gamma/T_s^2$ grow large. Precomputing these as constants (done at construction) avoids repeated division on the hot path and flags numerical range issues early.
+- **Stability boundary**: gains near $\beta = 4 - 2\alpha$ produce marginally stable responses. In practice, keep $\beta < 3 - 2\alpha$ for a margin of safety.
+- **Initialization**: the first sample seeds the position; velocity and acceleration are zero. Transient overshoot on a step input decays at a rate governed by the gains.
+- **Order-3 on a ramp**: the acceleration state will correctly settle near zero rather than accumulating a phantom bias, provided gains are stable.
+
+## Variants & Generalizations
+
+- **Order 2 ($\alpha$-$\beta$)**: tracks position and velocity; optimal for constant-velocity targets.
+- **Order 3 ($\alpha$-$\beta$-$\gamma$)**: adds acceleration; suitable for maneuvering targets but requires additional tuning of $\gamma$.
+- **Adaptive gains**: switching $\alpha$ between large (maneuver) and small (coast) values gives an interactive multiple-model (IMM) flavor without full Kalman complexity.
+- **Steady-state Kalman**: the $\alpha$-$\beta$ filter is exactly a scalar Kalman filter whose Riccati equation has converged, making $\lambda$ the natural design parameter.
+
+## Applications
+
+- **Radar / ranging**: smoothing noisy range or angle measurements while estimating radial velocity.
+- **Motor control**: fusing encoder position to estimate shaft velocity for a feedback loop.
+- **IMU pre-filtering**: attenuating high-frequency vibration before integrating acceleration.
+- **Any tight ISR**: when covariance propagation is too expensive but a plain IIR gives no velocity.
+
+## Connections to Other Algorithms
+
+- **KalmanFilter**: the $\alpha$-$\beta$ filter is its steady-state specialization; the full filter adapts gains to non-stationary noise.
+- **ExponentialMovingAverage**: position-only smoothing — no velocity estimate, equivalent to $\beta = 0$.
+- **ComplementaryFilter**: fuses two sensors in the frequency domain; similar predict/correct intuition but requires two measurement streams.
+
+## References & Further Reading
+
+- P. Kalata, "The tracking index: A generalized parameter for alpha-beta and alpha-beta-gamma target trackers," *IEEE Transactions on Aerospace and Electronic Systems*, 20(2), pp. 174–182, 1984.
+- S. Blackman and R. Popoli, *Design and Analysis of Modern Tracking Systems*, Artech House, 1999.
+- R. G. Brown and P. Y. C. Hwang, *Introduction to Random Signals and Applied Kalman Filtering*, 4th ed., Wiley, 2012.

numerical/filters/active/AlphaBetaFilter.cpp

@@ -0,0 +1,7 @@
+#include "numerical/filters/active/AlphaBetaFilter.hpp"
+
+namespace filters
+{
+    template class AlphaBetaFilter<float, 2>;
+    template class AlphaBetaFilter<float, 3>;
+}

numerical/filters/active/AlphaBetaFilter.hpp

@@ -0,0 +1,143 @@
+#pragma once
+
+#if defined(__GNUC__) || defined(__clang__)
+#pragma GCC optimize("O3", "fast-math")
+#endif
+
+#include "numerical/math/CompilerOptimizations.hpp"
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <type_traits>
+
+namespace filters
+{
+    template<typename T, std::size_t Order>
+    class AlphaBetaFilter
+    {
+        static_assert(std::is_floating_point_v<T>, "AlphaBetaFilter supports floating-point types");
+        static_assert(Order == 2 || Order == 3, "AlphaBetaFilter Order must be 2 or 3");
+
+    public:
+        struct Gains
+        {
+            T alpha;
+            T beta;
+        };
+
+        AlphaBetaFilter(T alpha, T beta, T Ts)
+        requires(Order == 2);
+
+        AlphaBetaFilter(T alpha, T beta, T gamma, T Ts)
+        requires(Order == 3);
+
+        OPTIMIZE_FOR_SPEED T Filter(T measuredPosition);
+
+        std::array<T, Order> State() const;
+
+        void Reset(T position = T{});
+
+        static Gains GainsFromTrackingIndex(T lambda);
+
+    private:
+        T samplePeriod{};
+        T gainAlpha{};
+        T gainBeta{};
+        T gainGamma{};
+        T betaOverTs{};
+        T twoGammaOverTs2{};
+        std::array<T, Order> state{};
+        bool initialized{ false };
+    };
+
+    // Implementation //
+
+    template<typename T, std::size_t Order>
+    AlphaBetaFilter<T, Order>::AlphaBetaFilter(T alpha, T beta, T Ts)
+    requires(Order == 2)
+        : samplePeriod{ Ts }
+        , gainAlpha{ alpha }
+        , gainBeta{ beta }
+        , gainGamma{ T{} }
+        , betaOverTs{ beta / Ts }
+        , twoGammaOverTs2{ T{} }
+    {}
+
+    template<typename T, std::size_t Order>
+    AlphaBetaFilter<T, Order>::AlphaBetaFilter(T alpha, T beta, T gamma, T Ts)
+    requires(Order == 3)
+        : samplePeriod{ Ts }
+        , gainAlpha{ alpha }
+        , gainBeta{ beta }
+        , gainGamma{ gamma }
+        , betaOverTs{ beta / Ts }
+        , twoGammaOverTs2{ T{ 2 } * gamma / (Ts * Ts) }
+    {}
+
+    template<typename T, std::size_t Order>
+    OPTIMIZE_FOR_SPEED T AlphaBetaFilter<T, Order>::Filter(T measuredPosition)
+    {
+        if (!initialized)
+        {
+            state[0] = measuredPosition;
+            initialized = true;
+            return measuredPosition;
+        }
+
+        T predicted0{};
+        T predicted1{};
+        T predicted2{};
+
+        if constexpr (Order == 3)
+        {
+            predicted0 = state[0] + samplePeriod * state[1] + T{ 0.5 } * samplePeriod * samplePeriod * state[2];
+            predicted1 = state[1] + samplePeriod * state[2];
+            predicted2 = state[2];
+        }
+        else
+        {
+            predicted0 = state[0] + samplePeriod * state[1];
+            predicted1 = state[1];
+        }
+
+        T residual{ measuredPosition - predicted0 };
+
+        state[0] = predicted0 + gainAlpha * residual;
+        state[1] = predicted1 + betaOverTs * residual;
+
+        if constexpr (Order == 3)
+        {
+            state[2] = predicted2 + twoGammaOverTs2 * residual;
+        }
+
+        return state[0];
+    }
+
+    template<typename T, std::size_t Order>
+    std::array<T, Order> AlphaBetaFilter<T, Order>::State() const
+    {
+        return state;
+    }
+
+    template<typename T, std::size_t Order>
+    void AlphaBetaFilter<T, Order>::Reset(T position)
+    {
+        state = {};
+        state[0] = position;
+        initialized = false;
+    }
+
+    template<typename T, std::size_t Order>
+    typename AlphaBetaFilter<T, Order>::Gains AlphaBetaFilter<T, Order>::GainsFromTrackingIndex(T lambda)
+    {
+        T r{ (T{ 4 } + lambda - std::sqrt(T{ 8 } * lambda + lambda * lambda)) / T{ 4 } };
+        T alpha{ T{ 1 } - r * r };
+        T beta{ T{ 2 } * (T{ 2 } - alpha) - T{ 4 } * std::sqrt(T{ 1 } - alpha) };
+        return Gains{ alpha, beta };
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class AlphaBetaFilter<float, 2>;
+    extern template class AlphaBetaFilter<float, 3>;
+#endif
+}

numerical/filters/active/CMakeLists.txt

@@ -12,6 +12,7 @@ target_link_libraries(numerical.filters.active ${NUMERICAL_VISIBILITY}
 )
 
 target_sources(numerical.filters.active PRIVATE
+    AlphaBetaFilter.hpp
     ComplementaryFilter.hpp
     ExtendedKalmanFilter.hpp
     KalmanFilter.hpp
@@ -21,6 +22,7 @@ target_sources(numerical.filters.active PRIVATE
 )
 
 numerical_add_coverage_sources(numerical.filters.active
+    AlphaBetaFilter.cpp
     ComplementaryFilter.cpp
     ExtendedKalmanFilter.cpp
     KalmanFilter.cpp

numerical/filters/active/test/CMakeLists.txt

@@ -8,6 +8,7 @@ target_link_libraries(numerical.filters.active_test PUBLIC
 )
 
 target_sources(numerical.filters.active_test PRIVATE
+    TestAlphaBetaFilter.cpp
     TestComplementaryFilter.cpp
     TestExtendedKalmanFilter.cpp
     TestKalmanFilter.cpp