feat: improve robustness and modernize math algorithms

doc/solvers/DurandKerner.md

@@ -74,7 +74,7 @@ After ~15–20 iterations the roots converge to $z \approx \{1, 2, 3\}$ (imagina
 - **Repeated roots.** Convergence degrades from quadratic to linear. Higher tolerance or more iterations may be needed.
 - **Near-degenerate denominators.** When two root estimates are very close ($|z_r - z_j| < 10^{-15}$), the denominator product approaches zero. The implementation excludes such terms to avoid division by near-zero.
 - **Leading coefficient must be non-zero.** The polynomial degree is determined by the first coefficient.
-- **Complex arithmetic required.** This algorithm operates entirely in $\mathbb{C}$, so it is limited to floating-point types (`float`, `double`). Fixed-point types are not supported.
+- **Complex arithmetic required.** This algorithm operates entirely in $\mathbb{C}$, using `math::Complex<T>` (from `numerical/math/ComplexNumber.hpp`), so it is limited to floating-point types (`float`, `double`). Fixed-point types are not supported.
 - **No convergence guarantee for all polynomials.** Wilkinson's polynomial and other pathological cases may require higher precision or alternative methods.
 - **Root ordering.** Results are sorted by real part (ascending), which may not correspond to meaningful branch ordering in applications like [Root Locus](../control_analysis/RootLocus.md).
 

numerical/control_analysis/RootLocus.hpp

@@ -7,10 +7,10 @@
 #include "infra/util/BoundedVector.hpp"
 #include "infra/util/ReallyAssert.hpp"
 #include "numerical/math/CompilerOptimizations.hpp"
+#include "numerical/math/ComplexNumber.hpp"
 #include "numerical/solvers/DurandKerner.hpp"
 #include <array>
 #include <cmath>
-#include <complex>
 #include <cstddef>
 #include <numbers>
 #include <span>
@@ -27,9 +27,9 @@ namespace control_analysis
             "MaxGainSteps must be greater than 1");
 
     public:
-        using RootVector = typename infra::BoundedVector<std::complex<T>>::template WithMaxSize<MaxOrder>;
+        using RootVector = typename infra::BoundedVector<math::Complex<T>>::template WithMaxSize<MaxOrder>;
         using GainVector = typename infra::BoundedVector<T>::template WithMaxSize<MaxGainSteps>;
-        using LociBranch = typename infra::BoundedVector<std::complex<T>>::template WithMaxSize<MaxGainSteps>;
+        using LociBranch = typename infra::BoundedVector<math::Complex<T>>::template WithMaxSize<MaxGainSteps>;
 
         struct Result
         {

numerical/control_analysis/test/TestRootLocus.cpp

@@ -2,7 +2,6 @@
 #include "numerical/math/Tolerance.hpp"
 #include <array>
 #include <cmath>
-#include <complex>
 #include <gtest/gtest.h>
 #include <numbers>
 
@@ -23,8 +22,8 @@ TEST_F(TestRootLocus, open_loop_poles_first_order_plant)
     auto result = rootLocus.Calculate(num, den, 1.0f);
 
     ASSERT_EQ(result.openLoopPoles.size(), 1u);
-    EXPECT_NEAR(result.openLoopPoles[0].real(), -2.0f, math::Tolerance<float>());
-    EXPECT_NEAR(result.openLoopPoles[0].imag(), 0.0f, math::Tolerance<float>());
+    EXPECT_NEAR(result.openLoopPoles[0].Real(), -2.0f, math::Tolerance<float>());
+    EXPECT_NEAR(result.openLoopPoles[0].Imaginary(), 0.0f, math::Tolerance<float>());
 }
 
 TEST_F(TestRootLocus, open_loop_poles_second_order_underdamped)
@@ -37,10 +36,10 @@ TEST_F(TestRootLocus, open_loop_poles_second_order_underdamped)
     auto result = rootLocus.Calculate(num, den, 1.0f);
 
     ASSERT_EQ(result.openLoopPoles.size(), 2u);
-    EXPECT_NEAR(result.openLoopPoles[0].real(), -zeta * wn, 1e-2f);
-    EXPECT_NEAR(result.openLoopPoles[1].real(), -zeta * wn, 1e-2f);
+    EXPECT_NEAR(result.openLoopPoles[0].Real(), -zeta * wn, 1e-2f);
+    EXPECT_NEAR(result.openLoopPoles[1].Real(), -zeta * wn, 1e-2f);
     float expectedImag = wn * std::sqrt(1.0f - zeta * zeta);
-    EXPECT_NEAR(std::abs(result.openLoopPoles[0].imag()), expectedImag, 1e-2f);
+    EXPECT_NEAR(std::abs(result.openLoopPoles[0].Imaginary()), expectedImag, 1e-2f);
 }
 
 TEST_F(TestRootLocus, open_loop_zeros_identified)
@@ -51,8 +50,8 @@ TEST_F(TestRootLocus, open_loop_zeros_identified)
     auto result = rootLocus.Calculate(num, den, 1.0f);
 
     ASSERT_EQ(result.openLoopZeros.size(), 1u);
-    EXPECT_NEAR(result.openLoopZeros[0].real(), -3.0f, math::Tolerance<float>());
-    EXPECT_NEAR(result.openLoopZeros[0].imag(), 0.0f, math::Tolerance<float>());
+    EXPECT_NEAR(result.openLoopZeros[0].Real(), -3.0f, math::Tolerance<float>());
+    EXPECT_NEAR(result.openLoopZeros[0].Imaginary(), 0.0f, math::Tolerance<float>());
 }
 
 TEST_F(TestRootLocus, gain_sweep_step_count_and_active_branches)
@@ -88,10 +87,10 @@ TEST_F(TestRootLocus, closed_loop_poles_match_analytic_at_k1_doc_example)
     ASSERT_EQ(result.closedLoopPoles.size(), 2u);
     float r0 = (-3.0f - std::sqrt(5.0f)) / 2.0f;
     float r1 = (-3.0f + std::sqrt(5.0f)) / 2.0f;
-    EXPECT_NEAR(result.closedLoopPoles[0].real(), r0, 1e-2f);
-    EXPECT_NEAR(result.closedLoopPoles[0].imag(), 0.0f, 1e-2f);
-    EXPECT_NEAR(result.closedLoopPoles[1].real(), r1, 1e-2f);
-    EXPECT_NEAR(result.closedLoopPoles[1].imag(), 0.0f, 1e-2f);
+    EXPECT_NEAR(result.closedLoopPoles[0].Real(), r0, 1e-2f);
+    EXPECT_NEAR(result.closedLoopPoles[0].Imaginary(), 0.0f, 1e-2f);
+    EXPECT_NEAR(result.closedLoopPoles[1].Real(), r1, 1e-2f);
+    EXPECT_NEAR(result.closedLoopPoles[1].Imaginary(), 0.0f, 1e-2f);
 }
 
 TEST_F(TestRootLocus, current_gain_stored_in_result)
@@ -114,11 +113,11 @@ TEST_F(TestRootLocus, branches_start_near_open_loop_poles_at_low_gain)
     ASSERT_GE(result.loci[0].size(), 1u);
     ASSERT_GE(result.loci[1].size(), 1u);
 
-    float p0 = result.openLoopPoles[0].real();
-    float p1 = result.openLoopPoles[1].real();
+    float p0 = result.openLoopPoles[0].Real();
+    float p1 = result.openLoopPoles[1].Real();
 
-    float loci0Start = result.loci[0].front().real();
-    float loci1Start = result.loci[1].front().real();
+    float loci0Start = result.loci[0].front().Real();
+    float loci1Start = result.loci[1].front().Real();
 
     bool branch0NearP0 = std::abs(loci0Start - p0) < 0.5f;
     bool branch0NearP1 = std::abs(loci0Start - p1) < 0.5f;
@@ -140,11 +139,11 @@ TEST_F(TestRootLocus, asymptote_centroid_three_poles_one_zero)
 
     float sumPoles = 0.0f;
     for (const auto& p : result.openLoopPoles)
-        sumPoles += p.real();
+        sumPoles += p.Real();
 
     float sumZeros = 0.0f;
     for (const auto& z : result.openLoopZeros)
-        sumZeros += z.real();
+        sumZeros += z.Real();
 
     float centroid = (sumPoles - sumZeros) / static_cast<float>(result.openLoopPoles.size() - result.openLoopZeros.size());
 
@@ -178,7 +177,7 @@ TEST_F(TestRootLocus, loci_move_left_with_increasing_gain_first_order)
     auto result = rootLocus.Calculate(num, den, 1.0f, 0.1f, 100.0f);
 
     ASSERT_GE(result.loci[0].size(), 2u);
-    EXPECT_LT(result.loci[0].back().real(), result.loci[0].front().real());
+    EXPECT_LT(result.loci[0].back().Real(), result.loci[0].front().Real());
 }
 
 TEST_F(TestRootLocus, second_order_poles_become_complex_at_high_gain)
@@ -193,15 +192,15 @@ TEST_F(TestRootLocus, second_order_poles_become_complex_at_high_gain)
     bool foundComplex = false;
     for (const auto& root : result.loci[0])
     {
-        if (std::abs(root.imag()) > 0.1f)
+        if (std::abs(root.Imaginary()) > 0.1f)
         {
             foundComplex = true;
             break;
         }
     }
     for (const auto& root : result.loci[1])
     {
-        if (std::abs(root.imag()) > 0.1f)
+        if (std::abs(root.Imaginary()) > 0.1f)
         {
             foundComplex = true;
             break;
@@ -222,12 +221,12 @@ TEST_F(TestRootLocus, conjugate_symmetry_when_complex_pair_present)
     bool verified = false;
     for (std::size_t i = 0; i < result.loci[0].size(); ++i)
     {
-        float im0 = result.loci[0][i].imag();
-        float im1 = result.loci[1][i].imag();
+        float im0 = result.loci[0][i].Imaginary();
+        float im1 = result.loci[1][i].Imaginary();
         if (std::abs(im0) > 0.1f && std::abs(im1) > 0.1f)
         {
-            float re0 = result.loci[0][i].real();
-            float re1 = result.loci[1][i].real();
+            float re0 = result.loci[0][i].Real();
+            float re1 = result.loci[1][i].Real();
             EXPECT_NEAR(re0, re1, 1e-2f);
             EXPECT_NEAR(im0, -im1, 1e-2f);
             verified = true;
@@ -244,7 +243,7 @@ TEST_F(TestRootLocus, closed_loop_poles_in_lhp_for_stable_gain)
     auto result = rootLocus.Calculate(num, den, 1.0f);
 
     for (const auto& p : result.closedLoopPoles)
-        EXPECT_LT(p.real(), 0.0f);
+        EXPECT_LT(p.Real(), 0.0f);
 }
 
 TEST_F(TestRootLocus, all_loci_points_are_finite)
@@ -258,8 +257,8 @@ TEST_F(TestRootLocus, all_loci_points_are_finite)
     {
         for (const auto& pt : result.loci[b])
         {
-            EXPECT_TRUE(std::isfinite(pt.real()));
-            EXPECT_TRUE(std::isfinite(pt.imag()));
+            EXPECT_TRUE(std::isfinite(pt.Real()));
+            EXPECT_TRUE(std::isfinite(pt.Imaginary()));
         }
     }
 }
@@ -278,8 +277,8 @@ TEST_F(TestRootLocus, determinism_same_input_same_output)
         ASSERT_EQ(result1.loci[b].size(), result2.loci[b].size());
         for (std::size_t i = 0; i < result1.loci[b].size(); ++i)
         {
-            EXPECT_FLOAT_EQ(result1.loci[b][i].real(), result2.loci[b][i].real());
-            EXPECT_FLOAT_EQ(result1.loci[b][i].imag(), result2.loci[b][i].imag());
+            EXPECT_FLOAT_EQ(result1.loci[b][i].Real(), result2.loci[b][i].Real());
+            EXPECT_FLOAT_EQ(result1.loci[b][i].Imaginary(), result2.loci[b][i].Imaginary());
         }
     }
 }
@@ -315,6 +314,6 @@ TEST_F(TestRootLocus, closed_loop_pole_first_order_analytic)
     auto result = rootLocus.Calculate(num, den, 2.0f);
 
     ASSERT_EQ(result.closedLoopPoles.size(), 1u);
-    EXPECT_NEAR(result.closedLoopPoles[0].real(), -3.0f, math::Tolerance<float>());
-    EXPECT_NEAR(result.closedLoopPoles[0].imag(), 0.0f, math::Tolerance<float>());
+    EXPECT_NEAR(result.closedLoopPoles[0].Real(), -3.0f, math::Tolerance<float>());
+    EXPECT_NEAR(result.closedLoopPoles[0].Imaginary(), 0.0f, math::Tolerance<float>());
 }

numerical/filters/active/test/CMakeLists.txt

@@ -5,6 +5,7 @@ emil_add_test(numerical.filters.active_test)
 target_link_libraries(numerical.filters.active_test PUBLIC
     gmock_main
     numerical.filters.active
+    numerical.math_test_helper
 )
 
 target_sources(numerical.filters.active_test PRIVATE

numerical/filters/active/test/TestExtendedKalmanFilter.cpp

@@ -1,22 +1,13 @@
 #include "numerical/filters/active/ExtendedKalmanFilter.hpp"
 #include "numerical/math/Tolerance.hpp"
+#include "numerical/math/test_doubles/MatrixTestSupport.hpp"
 #include <gtest/gtest.h>
 
 namespace
 {
-    template<typename T, std::size_t Size>
-    bool AreVectorsNear(const math::Vector<T, Size>& a,
-        const math::Vector<T, Size>& b,
-        float epsilon)
-    {
-        for (std::size_t i = 0; i < Size; ++i)
-            if (std::abs(math::ToFloat(a.at(i, 0)) - math::ToFloat(b.at(i, 0))) >= epsilon)
-                return false;
-
-        return true;
-    }
+    using math::test::AreVectorsNear;
 
-    // Linear state transition: x_new = F * x (constant velocity model)
+    using StateVec2 = math::Vector<float, 2>;
     using StateVec2 = math::Vector<float, 2>;
     using MeasVec1 = math::Vector<float, 1>;
     using StateMat2 = math::SquareMatrix<float, 2>;

numerical/filters/active/test/TestKalmanFilter.cpp

@@ -1,34 +1,13 @@
 #include "numerical/filters/active/KalmanFilter.hpp"
 #include "numerical/math/LinearTimeInvariant.hpp"
 #include "numerical/math/Tolerance.hpp"
+#include "numerical/math/test_doubles/MatrixTestSupport.hpp"
 #include <gtest/gtest.h>
 
 namespace
 {
-    template<typename T, std::size_t Size>
-    bool AreVectorsNear(const math::Vector<T, Size>& a,
-        const math::Vector<T, Size>& b,
-        float epsilon)
-    {
-        for (std::size_t i = 0; i < Size; ++i)
-            if (std::abs(math::ToFloat(a.at(i, 0)) - math::ToFloat(b.at(i, 0))) >= epsilon)
-                return false;
-
-        return true;
-    }
-
-    template<typename T, std::size_t Rows, std::size_t Cols>
-    bool AreMatricesNear(const math::Matrix<T, Rows, Cols>& a,
-        const math::Matrix<T, Rows, Cols>& b,
-        float epsilon)
-    {
-        for (std::size_t i = 0; i < Rows; ++i)
-            for (std::size_t j = 0; j < Cols; ++j)
-                if (std::abs(math::ToFloat(a.at(i, j)) - math::ToFloat(b.at(i, j))) >= epsilon)
-                    return false;
-
-        return true;
-    }
+    using math::test::AreMatricesNear;
+    using math::test::AreVectorsNear;
 
     template<typename T>
     class KalmanFilterTest

numerical/math/CMakeLists.txt

@@ -42,6 +42,8 @@ numerical_add_coverage_sources(numerical.math
     MatrixExponential.cpp
     QNumber.cpp
     Quaternion.cpp
+    RecursiveBuffer.cpp
+    Toeplitz.cpp
 )
 
 add_subdirectory(test)

numerical/math/ComplexNumber.hpp

@@ -1,5 +1,7 @@
 #pragma once
 #include "numerical/math/QNumber.hpp"
+#include <cmath>
+#include <type_traits>
 
 namespace math
 {
@@ -120,6 +122,13 @@
         return Complex(-real, -imag);
     }
 
+    template<typename QNumberType>
+    std::enable_if_t<std::is_floating_point_v<QNumberType>, QNumberType>
+    Abs(const Complex<QNumberType>& c)
+    {
+        return std::hypot(c.Real(), c.Imaginary());
+    }
+
 #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
     extern template class Complex<float>;
     extern template class Complex<Q15>;

numerical/math/LinearTimeInvariant.hpp

@@ -6,17 +6,9 @@
 
 #include "numerical/math/CompilerOptimizations.hpp"
 #include "numerical/math/Matrix.hpp"
-#include "numerical/math/QNumber.hpp"
 
 namespace math
 {
-    // Discrete-time linear time-invariant state-space model:
-    //   x_{k+1} = A x_k + B u_k
-    //   y_k     = C x_k + D u_k
-    //
-    // Template args: <T, StateSize=n, InputSize=m, OutputSize=p>
-    // OutputSize defaults to StateSize. All matrices are zero-initialised by default;
-    // use the WithFullStateOutput factory to get C = I.
     template<typename T,
         std::size_t StateSize,
         std::size_t InputSize,
@@ -45,19 +37,16 @@ namespace math
         OutputMatrix C{};
         FeedthroughMatrix D{};
 
-        // x_{k+1} = A x_k + B u_k
         ALWAYS_INLINE_HOT StateVector Step(const StateVector& x, const InputVector& u) const
         {
             return A * x + B * u;
         }
 
-        // y_k = C x_k + D u_k
         ALWAYS_INLINE_HOT OutputVector Output(const StateVector& x, const InputVector& u) const
         {
             return C * x + D * u;
         }
 
-        // Factory: C = I (OutputSize must equal StateSize), D = 0
         static LinearTimeInvariant WithFullStateOutput(
             const StateMatrix& stateTransition, const InputMatrix& inputMatrix)
         requires(OutputSize == StateSize)
@@ -69,7 +58,6 @@ namespace math
             return lti;
         }
 
-        // Factory: autonomous system (B = 0, D = 0)
         static LinearTimeInvariant Autonomous(
             const StateMatrix& stateTransition, const OutputMatrix& outputMatrix)
         {