feat: add complementary filter (#172)

* add complementary filter

* 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, FIR, IIR, Exponential Moving Average, Moving Average, Median Filter |
+| [Filters](doc/filters/README.md)                                   | Kalman, Extended Kalman, Unscented Kalman, Complementary, FIR, IIR, Exponential Moving Average, Moving Average, 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

@@ -28,7 +28,6 @@ Difficulty legend:
 | #  | Component                                            | Target module             | Difficulty |
 |----|------------------------------------------------------|---------------------------|------------|
 | 8  | Alpha-beta / alpha-beta-gamma filter                 | `filters/active`          | ★★☆☆☆      |
-| 9  | Complementary filter                                 | `filters/active`          | ★★☆☆☆      |
 | 10 | Gain-scheduled controller                            | `controllers`             | ★★☆☆☆      |
 | 11 | Convolution & correlation utilities                  | `analysis`                | ★★☆☆☆      |
 | 12 | Polynomial least-squares curve fitting               | `estimators/offline`      | ★★☆☆☆      |
@@ -106,12 +105,6 @@ Difficulty legend:
 - **Algorithm / paper:** P. Kalata, "The tracking index: A generalized parameter for α-β and α-β-γ target trackers," *IEEE Trans. AES*, 20(2), 1984.
 - **Reuses:** `math::LinearTimeInvariant`, `filters/active` patterns.
 
-### 9. Complementary filter
-- **What:** Frequency-domain fusion of a fast/biased sensor (gyro) with a slow/noisy one (accel/mag): `θ = a·(θ+ω·Ts) + (1−a)·θ_accel`.
-- **Embedded value:** The default low-cost attitude/tilt estimator on every IMU-based project.
-- **Algorithm / paper:** W. T. Higgins, "A Comparison of Complementary and Kalman Filtering," *IEEE Trans. AES*, 11(3), 1975.
-- **Reuses:** `math::Geometry3D`, item 1.
-
 ### 10. Gain-scheduled controller
 - **What:** Interpolates a set of precomputed controller gains across a scheduling variable (speed, load, operating point).
 - **Embedded value:** Extends linear controllers to mildly nonlinear plants without online redesign.

doc/filters/active/ComplementaryFilter.md

@@ -0,0 +1,122 @@
+# Complementary Filter
+
+## Overview & Motivation
+
+Real-time attitude and heading estimation requires fusing two fundamentally different sensor
+modalities: a **gyroscope** that integrates angular rate to produce a short-term angle estimate
+(fast response, low noise, but subject to drift) and an **accelerometer or magnetometer** that
+reads the angle directly (accurate at rest, noisy during motion, slow dynamics). Neither sensor
+alone is sufficient. The complementary filter solves the fusion problem with two multiplies
+and two adds per sample, making it the default tilt estimator on virtually every IMU-based
+embedded project where a full Kalman filter is unaffordable.
+
+## Mathematical Theory
+
+### Frequency-Domain Complement
+
+The two sensor paths are complementary in the transfer-function sense: the gyro path acts as a
+**first-order high-pass filter** and the direct-angle path acts as a **first-order low-pass filter**
+sharing the same crossover frequency $\omega_c = 1/\tau$. Their sum is identically unity for all
+frequencies:
+
+$$H_{HP}(s) + H_{LP}(s) = 1$$
+
+This ensures that no frequency content is amplified or attenuated by the fusion itself.
+
+### Discrete-Time Update Equation
+
+Given the fused angle $\theta_k$, gyro rate $\omega_k$, accelerometer angle $\theta_{acc,k}$,
+sample period $T_s$, and blend weight $\alpha \in [0,1]$:
+
+$$\theta_{k+1} = \alpha\,(\theta_k + \omega_k\,T_s) + (1-\alpha)\,\theta_{acc,k}$$
+
+The term $\theta_k + \omega_k T_s$ is the **high-pass path** (integration of the fast sensor),
+and $\theta_{acc,k}$ is the **low-pass path** (direct measurement from the slow sensor).
+
+### Design Parameter
+
+The single tuning knob is the crossover time constant $\tau$, which maps to $\alpha$ via:
+
+$$\alpha = \frac{\tau}{\tau + T_s}$$
+
+Below $1/\tau$ the filter trusts the accelerometer; above it, the gyroscope. Typical embedded
+values are $\tau = 0.5$–$2$ s, corresponding to $\alpha \approx 0.98$ at $T_s = 10$ ms.
+
+### Heading Wrap (Shortest-Arc Blend)
+
+When the state is a heading angle in $(-\pi, \pi]$, a naive linear blend can jump by $2\pi$ near
+the seam. Instead the blend is performed along the shortest arc:
+
+$$\delta = \mathrm{WrapToPi}(\theta_{acc} - \hat{\theta})$$
+$$\theta_{k+1} = \mathrm{WrapToPi}\!\left(\hat{\theta} + (1-\alpha)\,\delta\right)$$
+
+where $\mathrm{WrapToPi}(x) = \bigl((x + \pi) \bmod 2\pi\bigr) - \pi$.
+
+## Complexity Analysis
+
+| Operation | Time   | Space  | Notes                                      |
+|-----------|--------|--------|--------------------------------------------|
+| Update    | $O(1)$ | $O(1)$ | 2 multiplies, 2 adds; 1 fmod when wrapping |
+| Reset     | $O(1)$ | $O(1)$ | Single state write                         |
+
+Total storage: one angle word plus two constant coefficients.
+
+## Step-by-Step Walkthrough
+
+**Setup:** $\alpha = 0.98$, $T_s = 10$ ms, initial angle $= 0$.
+
+**Sample 1:** gyro rate $\omega = 10$ deg/s $= 0.1745$ rad/s, accel reads $\theta_{acc} = 0.01$ rad.
+
+$$\hat{\theta} = 0 + 0.1745 \times 0.01 = 0.001745 \text{ rad} \quad (\text{gyro path})$$
+$$\theta_1 = 0.98 \times 0.001745 + 0.02 \times 0.01 = 0.001710 + 0.000200 = 0.001910 \text{ rad}$$
+
+**After many samples with zero rate and accel $= 0.2$ rad:** the low-pass term accumulates
+and $\theta \to 0.2$ rad as $(1-\alpha)^n \to 0$.
+
+## Pitfalls & Edge Cases
+
+- **Alpha at 1:** the gyro path integrates without bound; any constant bias drifts the angle
+  indefinitely. This is intentional (gyro-only mode) but must be avoided in production.
+- **Alpha at 0:** the output equals the accelerometer reading at every step; the gyro is ignored.
+- **Heading wrap:** linear blending without shortest-arc correction produces a $2\pi$ jump when
+  the heading crosses $\pm\pi$. Always enable the wrap mode for heading estimation.
+- **Gyro bias:** the filter has no bias estimator. A constant gyro bias produces a bounded
+  steady-state error of approximately $\text{bias} \cdot T_s \cdot \alpha / (1-\alpha)$.
+  Pre-subtract a calibrated bias before calling Update.
+- **Accelerometer noise during dynamics:** the accel path is unreliable when linear acceleration
+  is present (non-gravitational). Reduce $(1-\alpha)$ or temporarily freeze the accel correction.
+
+## Variants & Generalizations
+
+| Variant                    | Key Difference                                                        |
+|----------------------------|-----------------------------------------------------------------------|
+| **Mahony filter**          | 3-D quaternion formulation with integral gyro-bias estimator          |
+| **Madgwick filter**        | Gradient-descent quaternion fusion; no linearisation                  |
+| **Alpha-Beta filter**      | Fixed-gain tracking without a slow sensor; pure high-pass integration |
+| **Kalman filter**          | Optimal (minimum-variance) fusion; requires noise covariance tuning   |
+| **Two-step complementary** | Separate pitch/roll from heading; common on 6-DOF IMUs                |
+
+## Applications
+
+- **IMU tilt estimation** — Roll and pitch from a 6-axis MEMS sensor at low computational cost.
+- **Heading fusion** — Combining gyro yaw rate with magnetometer heading.
+- **Servo/motor feedback** — Fusing encoder velocity with potentiometer position.
+- **Altitude hold** — Mixing barometer (low-pass) with accelerometer integration (high-pass).
+
+## Connections to Other Algorithms
+
+| Algorithm                                                            | Relationship                                                             |
+|----------------------------------------------------------------------|--------------------------------------------------------------------------|
+| [Exponential Moving Average](../passive/ExponentialMovingAverage.md) | The low-pass path in isolation; $\alpha_{EMA} = 1-\alpha_{CF}$           |
+| [Alpha-Beta Filter](AlphaBetaFilter.md)                              | Complementary filter without a slow-sensor reference; fixed-gain tracker |
+| [AHRS Madgwick/Mahony](AhrsMadgwickMahony.md)                        | 3-D quaternion generalization with gyro-bias estimation                  |
+| [Kalman Filter](KalmanFilter.md)                                     | Statistically optimal generalization requiring $Q$ and $R$ tuning        |
+
+## References & Further Reading
+
+- W. T. Higgins, "A Comparison of Complementary and Kalman Filtering," *IEEE Transactions on
+  Aerospace and Electronic Systems*, 11(3), pp. 321–325, 1975.
+- S. Madgwick, "An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays,"
+  Technical Report, University of Bristol, 2010.
+- R. Mahony, T. Hamel, and J.-M. Pflimlin, "Nonlinear Complementary Filters on the Special
+  Orthogonal Group," *IEEE Transactions on Automatic Control*, 53(5), pp. 1203–1218, 2008.

numerical/filters/active/CMakeLists.txt

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

numerical/filters/active/ComplementaryFilter.cpp

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

numerical/filters/active/ComplementaryFilter.hpp

@@ -0,0 +1,100 @@
+#pragma once
+
+#if defined(__GNUC__) || defined(__clang__)
+#pragma GCC optimize("O3", "fast-math")
+#endif
+
+#include "numerical/math/CompilerOptimizations.hpp"
+#include <cassert>
+#include <cmath>
+#include <numbers>
+#include <type_traits>
+
+namespace filters
+{
+    template<typename T>
+    class ComplementaryFilter
+    {
+        static_assert(std::is_floating_point_v<T>, "ComplementaryFilter supports floating-point types");
+
+    public:
+        explicit ComplementaryFilter(T alpha, T Ts, T initial = T{}, bool wrapAngle = false) noexcept;
+
+        OPTIMIZE_FOR_SPEED T Update(T rate, T measuredAngle) noexcept;
+        void Reset(T angle = T{}) noexcept;
+        void SetAlpha(T alpha) noexcept;
+
+        static T AlphaFromTau(T tau, T Ts) noexcept;
+
+    private:
+        T alpha;
+        T Ts;
+        T angle;
+        bool wrapAngle;
+
+        static T WrapToPi(T x) noexcept;
+    };
+
+    ////    Implementation    ////
+
+    template<typename T>
+    ComplementaryFilter<T>::ComplementaryFilter(T alpha, T Ts, T initial, bool wrapAngle) noexcept
+        : alpha{ alpha }
+        , Ts{ Ts }
+        , angle{ initial }
+        , wrapAngle{ wrapAngle }
+    {
+        assert(alpha >= T{} && alpha <= T{ 1 });
+        assert(Ts > T{});
+    }
+
+    template<typename T>
+    OPTIMIZE_FOR_SPEED T ComplementaryFilter<T>::Update(T rate, T measuredAngle) noexcept
+    {
+        T predicted{ angle + rate * Ts };
+        if (wrapAngle)
+        {
+            T delta{ WrapToPi(measuredAngle - predicted) };
+            angle = WrapToPi(predicted + (T{ 1 } - alpha) * delta);
+        }
+        else
+        {
+            angle = alpha * predicted + (T{ 1 } - alpha) * measuredAngle;
+        }
+        return angle;
+    }
+
+    template<typename T>
+    void ComplementaryFilter<T>::Reset(T value) noexcept
+    {
+        angle = value;
+    }
+
+    template<typename T>
+    void ComplementaryFilter<T>::SetAlpha(T newAlpha) noexcept
+    {
+        assert(newAlpha >= T{} && newAlpha <= T{ 1 });
+        alpha = newAlpha;
+    }
+
+    template<typename T>
+    T ComplementaryFilter<T>::AlphaFromTau(T tau, T Ts) noexcept
+    {
+        return tau / (tau + Ts);
+    }
+
+    template<typename T>
+    T ComplementaryFilter<T>::WrapToPi(T x) noexcept
+    {
+        constexpr T pi{ std::numbers::pi_v<T> };
+        constexpr T twoPi{ T{ 2 } * std::numbers::pi_v<T> };
+        T wrapped{ std::fmod(x + pi, twoPi) };
+        if (wrapped < T{})
+            wrapped += twoPi;
+        return wrapped - pi;
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class ComplementaryFilter<float>;
+#endif
+}

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
+    TestComplementaryFilter.cpp
     TestExtendedKalmanFilter.cpp
     TestKalmanFilter.cpp
     TestKalmanSmoother.cpp