diff --git a/README.md b/README.md index 7ae55c52..645e15d0 100644 --- a/README.md +++ b/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 | diff --git a/ROADMAP.md b/ROADMAP.md index 9c43e759..1030ec76 100644 --- a/ROADMAP.md +++ b/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` | ★★☆☆☆ | diff --git a/doc/filters/active/AlphaBetaFilter.md b/doc/filters/active/AlphaBetaFilter.md new file mode 100644 index 00000000..314e3845 --- /dev/null +++ b/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. diff --git a/numerical/filters/active/AlphaBetaFilter.cpp b/numerical/filters/active/AlphaBetaFilter.cpp new file mode 100644 index 00000000..61537c75 --- /dev/null +++ b/numerical/filters/active/AlphaBetaFilter.cpp @@ -0,0 +1,7 @@ +#include "numerical/filters/active/AlphaBetaFilter.hpp" + +namespace filters +{ + template class AlphaBetaFilter; + template class AlphaBetaFilter; +} diff --git a/numerical/filters/active/AlphaBetaFilter.hpp b/numerical/filters/active/AlphaBetaFilter.hpp new file mode 100644 index 00000000..633148ce --- /dev/null +++ b/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 +#include +#include +#include + +namespace filters +{ + template + class AlphaBetaFilter + { + static_assert(std::is_floating_point_v, "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 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 state{}; + bool initialized{ false }; + }; + + // Implementation // + + template + AlphaBetaFilter::AlphaBetaFilter(T alpha, T beta, T Ts) + requires(Order == 2) + : samplePeriod{ Ts } + , gainAlpha{ alpha } + , gainBeta{ beta } + , gainGamma{ T{} } + , betaOverTs{ beta / Ts } + , twoGammaOverTs2{ T{} } + {} + + template + AlphaBetaFilter::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 + OPTIMIZE_FOR_SPEED T AlphaBetaFilter::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 + std::array AlphaBetaFilter::State() const + { + return state; + } + + template + void AlphaBetaFilter::Reset(T position) + { + state = {}; + state[0] = position; + initialized = false; + } + + template + typename AlphaBetaFilter::Gains AlphaBetaFilter::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; + extern template class AlphaBetaFilter; +#endif +} diff --git a/numerical/filters/active/CMakeLists.txt b/numerical/filters/active/CMakeLists.txt index 228aea3a..1ef7899a 100644 --- a/numerical/filters/active/CMakeLists.txt +++ b/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 diff --git a/numerical/filters/active/test/CMakeLists.txt b/numerical/filters/active/test/CMakeLists.txt index 2737c78e..e993cc98 100644 --- a/numerical/filters/active/test/CMakeLists.txt +++ b/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 diff --git a/numerical/filters/active/test/TestAlphaBetaFilter.cpp b/numerical/filters/active/test/TestAlphaBetaFilter.cpp new file mode 100644 index 00000000..8f971e6b --- /dev/null +++ b/numerical/filters/active/test/TestAlphaBetaFilter.cpp @@ -0,0 +1,132 @@ +#include "numerical/filters/active/AlphaBetaFilter.hpp" +#include "numerical/math/Tolerance.hpp" +#include +#include + +namespace +{ + class TestAlphaBetaFilter + : public ::testing::Test + { + protected: + filters::AlphaBetaFilter filter{ 0.5f, 0.1f, 1.0f }; + }; + + class TestAlphaBetaGammaFilter + : public ::testing::Test + { + protected: + filters::AlphaBetaFilter filter{ 0.5f, 0.1f, 0.01f, 1.0f }; + }; +} + +TEST_F(TestAlphaBetaFilter, first_sample_seeds_position) +{ + float result{ filter.Filter(0.4f) }; + + EXPECT_NEAR(result, 0.4f, math::Tolerance()); + EXPECT_NEAR(filter.State()[1], 0.0f, math::Tolerance()); +} + +TEST_F(TestAlphaBetaFilter, constant_position_zero_velocity) +{ + for (int i = 0; i < 50; ++i) + filter.Filter(0.3f); + + EXPECT_NEAR(filter.State()[0], 0.3f, 1e-3f); + EXPECT_NEAR(filter.State()[1], 0.0f, 1e-3f); +} + +TEST_F(TestAlphaBetaFilter, constant_velocity_tracks_ramp) +{ + constexpr float v0{ 0.2f }; + constexpr float Ts{ 1.0f }; + + for (int n = 0; n < 200; ++n) + filter.Filter(v0 * static_cast(n) * Ts); + + EXPECT_NEAR(filter.State()[1], v0, 1e-2f); + EXPECT_NEAR(filter.State()[0], v0 * 199.0f * Ts, 1.0f); +} + +TEST_F(TestAlphaBetaFilter, step_response_settles) +{ + for (int i = 0; i < 100; ++i) + filter.Filter(1.0f); + + EXPECT_NEAR(filter.State()[0], 1.0f, 1e-2f); +} + +TEST_F(TestAlphaBetaFilter, gains_from_tracking_index_are_stable) +{ + auto g{ filters::AlphaBetaFilter::GainsFromTrackingIndex(0.5f) }; + + EXPECT_GT(g.alpha, 0.0f); + EXPECT_LT(g.alpha, 1.0f); + EXPECT_GT(g.beta, 0.0f); + EXPECT_LT(g.beta, 4.0f - 2.0f * g.alpha); +} + +TEST_F(TestAlphaBetaFilter, noise_is_attenuated) +{ + constexpr int N{ 200 }; + constexpr float noiseAmplitude{ 0.1f }; + + float inputVariance{ 0.0f }; + float outputVariance{ 0.0f }; + float inputMean{ 1.0f }; + float outputMean{ 0.0f }; + + std::array outputs{}; + for (int i = 0; i < N; ++i) + { + float noise{ noiseAmplitude * (i % 2 == 0 ? 1.0f : -1.0f) }; + float meas{ inputMean + noise }; + outputs[static_cast(i)] = filter.Filter(meas); + } + + for (int i = N / 2; i < N; ++i) + outputMean += outputs[static_cast(i)]; + outputMean /= static_cast(N / 2); + + for (int i = N / 2; i < N; ++i) + { + float od{ outputs[static_cast(i)] - outputMean }; + outputVariance += od * od; + } + outputVariance /= static_cast(N / 2); + + inputVariance = noiseAmplitude * noiseAmplitude; + + EXPECT_LT(outputVariance, inputVariance); +} + +TEST_F(TestAlphaBetaFilter, reset_clears_state) +{ + for (int i = 0; i < 10; ++i) + filter.Filter(5.0f); + + filter.Reset(0.0f); + + float result{ filter.Filter(0.7f) }; + + EXPECT_NEAR(result, 0.7f, math::Tolerance()); + EXPECT_NEAR(filter.State()[1], 0.0f, math::Tolerance()); +} + +TEST_F(TestAlphaBetaGammaFilter, alpha_beta_gamma_tracks_acceleration) +{ + constexpr float a0{ 0.2f }; + constexpr float Ts{ 1.0f }; + + for (int n = 0; n < 300; ++n) + { + float t{ static_cast(n) * Ts }; + filter.Filter(0.5f * a0 * t * t); + } + + EXPECT_NEAR(filter.State()[2], a0, 1e-1f); + float t299{ 299.0f * Ts }; + float expectedPos{ 0.5f * a0 * t299 * t299 }; + EXPECT_NEAR(filter.State()[0], expectedPos, expectedPos * 0.05f); +} diff --git a/roadmap/filters/active/AlphaBetaFilter/explanation.md b/roadmap/filters/active/AlphaBetaFilter/explanation.md deleted file mode 100644 index 12614ef2..00000000 --- a/roadmap/filters/active/AlphaBetaFilter/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# Alpha-Beta / Alpha-Beta-Gamma Filter — Overview - -## What it is -A fixed-gain predictor-corrector that tracks a target's **position and velocity** (and, in the -α-β-γ variant, **acceleration**). It has the same predict/update shape as a Kalman filter, but the -gains `α`, `β`, (`γ`) are **constants** instead of being recomputed from a covariance each step. - -## Why it matters (embedded) -It delivers most of the benefit of a steady-state Kalman filter at a tiny fraction of the cost: -no matrix inverse, no covariance propagation, just a few multiply-adds per sample. That makes it -the go-to smoother/tracker for radar, ranging, encoders, and any sensor where you need a clean -position **and** a velocity estimate inside a fast control loop. - -## How it works (intuition) -Each step **predicts** where the target should be using simple constant-velocity (or -constant-acceleration) kinematics, then **corrects** that prediction by a fraction of the -measurement residual. `α` controls how hard the position is pulled toward the measurement; `β` -does the same for velocity; `γ` for acceleration. Large gains → fast, noisy; small gains → -smooth, laggy. Kalata's *tracking index* ties the gains to the ratio of process noise to -measurement noise so a single scalar picks the whole set. - -## Key parameters -- **α** — position gain, `∈ (0, 1)`. -- **β** — velocity gain; stability needs `0 < β < 4 − 2α`. -- **γ** — acceleration gain (α-β-γ only), for maneuvering targets. -- **Ts** — sample period, links residual to velocity/acceleration units. -- **tracking index λ** — one knob that generates critically-damped gains. - -## Reference -P. Kalata, "The tracking index: A generalized parameter for α-β and α-β-γ target trackers," -*IEEE Trans. Aerospace and Electronic Systems*, 20(2), 1984. - -## See also -`KalmanFilter` (adaptive-gain generalization), `ComplementaryFilter` (frequency-domain fusion), -`ExponentialMovingAverage` (position-only smoothing). diff --git a/roadmap/filters/active/AlphaBetaFilter/implementation.md b/roadmap/filters/active/AlphaBetaFilter/implementation.md deleted file mode 100644 index 84681512..00000000 --- a/roadmap/filters/active/AlphaBetaFilter/implementation.md +++ /dev/null @@ -1,82 +0,0 @@ -# Alpha-Beta / Alpha-Beta-Gamma Filter — Implementation Pseudocode - -> Roadmap ref: #8 (Tier 2) · Target: `numerical/filters/active` · Namespace `filters` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class AlphaBetaFilter: # Order = 2 (α-β) or 3 (α-β-γ) - T Ts # sample period - T alpha, beta # position / velocity gains - T gamma # acceleration gain (Order == 3 only) - Vector state # [position, velocity, (acceleration)] - bool initialized = false -``` - -## Interface - -``` -AlphaBetaFilter(T alpha, T beta, T Ts) # Order == 2 -AlphaBetaFilter(T alpha, T beta, T gamma, T Ts) # Order == 3 -T Filter(T measuredPosition) # hot path: predict + correct, returns position -Vector State() const # full [pos, vel, (acc)] estimate -void Reset(T position = 0) -static Gains GainsFromTrackingIndex(T lambda) # Kalata steady-state design -``` - -## Algorithm (pseudocode) - -``` -function Filter(z): # OPTIMIZE_FOR_SPEED - if not initialized: # seed position, zero rates - state[0] = z; initialized = true; return z - - # --- Predict (constant-velocity / constant-accel kinematics) --- - p = state[0] + Ts*state[1] + (Order==3 ? 0.5*Ts*Ts*state[2] : 0) - v = state[1] + (Order==3 ? Ts*state[2] : 0) - a = state[2] # unchanged (Order == 3) - - # --- Correct with position residual --- - r = z - p # innovation - state[0] = p + alpha*r - state[1] = v + (beta / Ts)*r - if Order == 3: - state[2] = a + (2*gamma / (Ts*Ts))*r - return state[0] - -function GainsFromTrackingIndex(lambda): # α-β, Kalata 1984 - r = (4 + lambda - sqrt(8*lambda + lambda*lambda)) / 4 # smoothing root - alpha = 1 - r*r - beta = 2*(2 - alpha) - 4*sqrt(1 - alpha) - return {alpha, beta} -``` - -## Complexity & memory - -- Time: `O(1)` per sample — a handful of MACs, no matrix inverse, no covariance update. -- Memory: `O(Order)` — 2 or 3 state words plus the fixed gains. - -## Numerical / embedded notes - -- Deterministic drop-in for a steady-state Kalman filter: gains are precomputed, so there is - **no online covariance propagation** — ideal for tight ISR budgets. -- Stability (α-β) requires `0 < alpha < 1` and `0 < beta < 4 - 2*alpha` (Simpson's triangle). -- The **tracking index** `lambda = sqrt(process_var)*Ts² / sqrt(meas_var)` sets the smoothing/lag - trade-off; `GainsFromTrackingIndex` gives the critically-damped α-β gains from Kalata. -- Precompute `beta/Ts` and `2*gamma/Ts²` as constants so the hot path stays MAC-only. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/filters/active/AlphaBetaFilter.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Filter`, and - `extern template class AlphaBetaFilter;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/filters/active/AlphaBetaFilter.cpp` → - `template class AlphaBetaFilter;` -- Test: `numerical/filters/active/test/TestAlphaBetaFilter.cpp` -- Doc: `doc/filters/active/AlphaBetaFilter.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestAlphaBetaFilter.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/filters/active/AlphaBetaFilter/tests.md b/roadmap/filters/active/AlphaBetaFilter/tests.md deleted file mode 100644 index 7613b5c5..00000000 --- a/roadmap/filters/active/AlphaBetaFilter/tests.md +++ /dev/null @@ -1,59 +0,0 @@ -# Alpha-Beta / Alpha-Beta-Gamma Filter — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestAlphaBetaFilter : public ::testing::Test: - AlphaBetaFilter filter{ 0.5f, 0.1f, 1.0f } # alpha, beta, Ts -# each case below is a TEST_F(TestAlphaBetaFilter, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -first_sample_seeds_position: - Arrange: fresh filter - Act: y = Filter(0.4) - Assert: y == 0.4 and velocity == 0 - -constant_position_zero_velocity: - Arrange: feed constant z = 0.3 for 50 samples - Assert: position -> 0.3, velocity -> 0 (steady state, no lag) - -constant_velocity_tracks_ramp: - Arrange: z[n] = v0 * n * Ts (noise-free ramp), v0 = 0.2 - Assert: after settling, velocity ≈ v0 and position lag ≈ 0 - -step_response_settles: - Arrange: step z from 0 -> 1 - Assert: position converges to 1, monotone within stability bounds - -gains_from_tracking_index_are_stable: - Arrange: g = GainsFromTrackingIndex(0.5) - Assert: 0 < g.alpha < 1 and 0 < g.beta < 4 - 2*g.alpha - -noise_is_attenuated: - Arrange: constant true position + zero-mean noise - Assert: variance(output) < variance(input) - -reset_clears_state: - Arrange: run samples, Reset(0) - Assert: next Filter(z) seeds position = z, velocity = 0 - -alpha_beta_gamma_tracks_acceleration: # AlphaBetaFilter fixture - Arrange: z[n] = 0.5*a0*(n*Ts)^2 (constant-accel arc) - Assert: acceleration estimate ≈ a0, bounded position lag -``` - -## Reference vectors - -- Constant-velocity ramp: steady-state velocity estimate equals the true slope (zero lag). -- α-β Kalata design: `lambda = 1` ⇒ published `alpha`, `beta` pair reproduced by the helper. - -## Edge cases - -- `Ts` very small ⇒ `beta/Ts`, `2*gamma/Ts²` grow large: assert the estimate stays bounded. -- Gains at stability boundary (`beta -> 4 - 2*alpha`) ⇒ marginally stable, must not diverge. -- Order-3 filter fed a pure ramp ⇒ acceleration estimate stays ≈ 0 (no phantom accel).