add matrix exponential

Author: gabrielfrasantos

README.md

@@ -26,7 +26,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal
 | [Neural Network](doc/neural_network/README.md)                     | Layers, activations, losses, model                                   |
 | [Optimization](doc/optimization/README.md)                         | Gradient Descent                                                     |
 | [Regularization](doc/regularization/README.md)                     | L1 (Lasso), L2 (Ridge)                                              |
-| [Math](doc/math/README.md)                                         | CORDIC, Quaternion, MatrixNorms, Step Response Metrics                                                   |
+| [Math](doc/math/README.md)                                         | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential                                |
 | [Solvers](doc/solvers/README.md)                                   | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting |
 | [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations, SIMD                                         |
 

ROADMAP.md

@@ -27,7 +27,6 @@ Difficulty legend:
 
 | #  | Component                                            | Target module             | Difficulty |
 |----|------------------------------------------------------|---------------------------|------------|
-| 29 | Matrix exponential (scaling & squaring + Padé)       | `math`                    | ★★★★☆      |
 | 30 | Continuous → discrete conversion (`c2d`)             | `math`                    | ★★★★☆      |
 | 31 | Lyapunov / Sylvester equation solvers                | `solvers`                 | ★★★★☆      |
 | 32 | Transfer-function ↔ state-space conversion           | `control_analysis`        | ★★★★☆      |

doc/math/MatrixExponential.md

@@ -0,0 +1,102 @@
+# Matrix Exponential
+
+## Overview & Motivation
+
+The matrix exponential is the fundamental solution operator for linear time-invariant (LTI) systems. Given a square matrix $A$, the matrix exponential $e^A$ maps initial conditions of the ODE $\dot{x} = Ax$ to their exact solution at unit time: $x(1) = e^A x(0)$. In embedded control, it appears as the exact discretisation of a continuous-time state-space model — turning the pair $(A_c, B_c)$ into the discrete $(A_d, B_d)$ that an MCU actually executes. Accurate, bounded computation of $e^A$ is therefore a prerequisite for correct control deployment.
+
+## Mathematical Theory
+
+### Series Definition
+
+For a square matrix $A \in \mathbb{R}^{n \times n}$, the matrix exponential is defined by the power series
+
+$$e^A = \sum_{k=0}^{\infty} \frac{A^k}{k!} = I + A + \frac{A^2}{2!} + \frac{A^3}{3!} + \cdots$$
+
+The series converges for every finite matrix and every field of characteristic zero.
+
+### Scaling and Squaring
+
+Truncating the series directly is unreliable for large $\|A\|$ because the intermediate terms grow into a large "hump" before cancelling — a phenomenon of catastrophic cancellation. The scaling-and-squaring algorithm avoids this by exploiting the identity
+
+$$e^A = \left(e^{A/2^s}\right)^{2^s}$$
+
+Choose $s = \max\!\left(0, \lceil \log_2 \|A\|_\infty \rceil\right)$ so that $\|A/2^s\|_\infty \leq 1$, compute $e^{A/2^s}$ accurately on the shrunken argument, then recover $e^A$ by repeated squaring.
+
+### Padé Approximant
+
+For the scaled matrix $\hat{A} = A/2^s$, the exponential is approximated by the diagonal $(q, q)$ Padé rational function
+
+$$e^{\hat{A}} \approx R_q(\hat{A}) = D_q(\hat{A})^{-1} N_q(\hat{A})$$
+
+where $N_q$ and $D_q$ are matrix polynomials whose scalar counterparts are the numerator and denominator of the $(q,q)$ Padé approximant to $e^x$. The coefficients $c_k$ satisfy
+
+$$c_k = \frac{(2q - k)!\, q!}{(2q)!\, k!\, (q-k)!}, \quad k = 0, 1, \ldots, q$$
+
+For $q = 6$ the coefficients are $c_0 = 1$, $c_1 = \tfrac{1}{2}$, $c_2 = \tfrac{5}{44}$, $c_3 = \tfrac{1}{66}$, $c_4 = \tfrac{1}{792}$, $c_5 = \tfrac{1}{15840}$, $c_6 = \tfrac{1}{665280}$.
+
+The even/odd split halves the number of matrix multiplications:
+
+$$V = c_0 I + c_2 \hat{A}^2 + c_4 \hat{A}^4 + c_6 \hat{A}^6$$
+$$U = \hat{A}\!\left(c_1 I + c_3 \hat{A}^2 + c_5 \hat{A}^4\right)$$
+$$N_q = V + U, \quad D_q = V - U$$
+
+The ratio $D_q^{-1} N_q$ is evaluated by solving the linear system $D_q X = N_q$ column-by-column using LU factorisation with partial pivoting, never forming $D_q^{-1}$ explicitly.
+
+## Complexity Analysis
+
+| Phase                     | Time        | Space      | Notes                                              |
+|---------------------------|-------------|------------|----------------------------------------------------|
+| Infinity-norm             | $O(n^2)$    | $O(1)$     | Determines scaling exponent $s$                    |
+| Matrix powers $A^2, A^4, A^6$ | $3\,O(n^3)$ | $3 n^2$    | Even/odd split; three multiplications total        |
+| Polynomial evaluation     | $O(n^3)$    | $2 n^2$    | Horner-style accumulation of $U$ and $V$           |
+| LU solve ($D_q X = N_q$) | $O(n^3)$    | $n^2$      | One factorisation, $n$ substitution passes         |
+| Squaring ($s$ steps)      | $s\,O(n^3)$ | $n^2$      | At most $\lceil \log_2 \|A\|_\infty \rceil$ steps |
+| **Total**                 | $O((6+s)n^3)$ | $O(n^2)$ | Stack-only; no heap                                |
+
+## Step-by-Step Walkthrough
+
+Consider $A = \begin{bmatrix}0 & -1 \\ 1 & 0\end{bmatrix}$, a rotation generator with $\theta = 1$.
+
+1. **Norm**: $\|A\|_\infty = 1$, so $s = 0$ (no scaling needed).
+2. **Polynomial evaluation**: compute $\hat{A}^2 = -I$, $\hat{A}^4 = I$, $\hat{A}^6 = -I$. Then
+   $$V = (c_0 - c_2 + c_4 - c_6)I, \quad U = (c_1 - c_3 + c_5)\hat{A}$$
+3. **Padé ratio**: $R = D^{-1}N$ solved via LU.
+4. **Squaring**: none ($s = 0$).
+5. **Result**: matches $\begin{bmatrix}\cos 1 & -\sin 1 \\ \sin 1 & \cos 1\end{bmatrix}$ to within float tolerance.
+
+## Pitfalls & Edge Cases
+
+**Catastrophic cancellation without scaling**: naive Padé on a large argument produces entries that nearly cancel, magnifying rounding error. The scaling step ensures $\|\hat{A}\|_\infty \leq 1$ before the rational approximation is applied.
+
+**Nilpotent matrices**: the series terminates in finitely many terms. Scaling-and-squaring handles this transparently; the Padé approximant reduces to a truncated polynomial.
+
+**Stiff systems (large negative eigenvalues)**: the result is bounded because $e^{\lambda}$ with $\lambda \ll 0$ is near zero. Floating-point underflow may drive these entries to zero; this is physically correct and numerically harmless.
+
+**Singular denominator**: $D_q$ is singular only if $e^A$ has a pole, which the matrix exponential never does ($A$ finite $\Rightarrow$ $e^A$ invertible). For the Padé denominator this means near-singularity can occur only at pathological arguments; the LU pivoting detects and gracefully handles it in practice.
+
+**Scaling exponent overflow**: for a matrix with entries $\approx 10^{38}$ the exponent $s$ would be $\approx 126$, requiring 126 squarings. This is accepted behaviour; the algorithm remains correct but slow.
+
+## Variants & Generalizations
+
+**Higher-order Padé**: orders 8, 10, or 13 reduce the required scaling and improve accuracy for modest $\|A\|$. Higham's 2005 algorithm chooses the order adaptively. Order 6 is a reasonable default for embedded float arithmetic.
+
+**Schur decomposition pre-conditioning**: computing $e^A = Q e^T Q^\mathsf{T}$ (with $T$ upper-triangular Schur form) avoids the hump phenomenon entirely and permits reuse of the Schur factors. The additional cost is the Schur decomposition itself ($O(n^3)$) and is not justified for small embedded matrices.
+
+**Taylor series with Horner evaluation**: accurate only for $\|A\| \ll 1$; the scaling step achieves exactly this, making scaling-and-squaring a superset.
+
+## Applications
+
+- **Exact discretisation** (`ContinuousToDiscrete`): $A_d = e^{A_c \Delta t}$.
+- **Continuous-time Gramians**: the controllability Gramian $W_c = \int_0^\infty e^{At} B B^\mathsf{T} e^{A^\mathsf{T}t}\,\mathrm{d}t$ requires $e^A$ repeatedly.
+- **Lie-group integration**: for rigid-body dynamics the matrix exponential maps the Lie algebra $\mathfrak{so}(3)$ (skew-symmetric matrices) to the Lie group $SO(3)$ (rotation matrices).
+- **Linear ODE simulation**: $x(t) = e^{At} x_0$ evaluated on an MCU for trajectory preview.
+
+## Connections to Other Algorithms
+
+`TriangularSolve` (`SolveUnitLowerTriangular`, `SolveUpperTriangular`) performs the back-substitution steps of the LU solve inside `SolvePade`. `MatrixNorms::InfinityNorm` computes the scaling exponent. `ContinuousToDiscrete` (roadmap item 30) is the primary consumer of this algorithm.
+
+## References & Further Reading
+
+- C. Moler, C. Van Loan, "Nineteen Dubious Ways to Compute the Exponential of a Matrix, Twenty-Five Years Later," *SIAM Review*, 45(1), 2003.
+- N. J. Higham, "The Scaling and Squaring Method for the Matrix Exponential Revisited," *SIAM Journal on Matrix Analysis and Applications*, 26(4), 2005.
+- N. J. Higham, "Functions of Matrices: Theory and Computation," SIAM, 2008, Chapter 10.

doc/math/README.md

@@ -14,3 +14,4 @@ Core mathematical primitives for numerical computation.
 | [Triangular Solve](TriangularSolve.md)              | Upper-triangular back-substitution shared by Gaussian elimination and QR                       |
 | [Matrix Operations](MatrixOperations.md)            | Structural matrix utilities — `Symmetrize` (closest symmetric matrix)                          |
 | [Step Response Metrics](StepResponseMetrics.md)     | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector |
+| [Matrix Exponential](MatrixExponential.md)          | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine |

numerical/math/CMakeLists.txt

@@ -19,6 +19,7 @@ target_sources(numerical.math PRIVATE
     HyperbolicFunctions.hpp
     LinearTimeInvariant.hpp
     Matrix.hpp
+    MatrixExponential.hpp
     MatrixNorms.hpp
     MatrixOperations.hpp
     QNumber.hpp
@@ -38,6 +39,7 @@ numerical_add_coverage_sources(numerical.math
     Cordic.cpp
     LinearTimeInvariant.cpp
     Matrix.cpp
+    MatrixExponential.cpp
     QNumber.cpp
     Quaternion.cpp
 )

numerical/math/MatrixExponential.cpp

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

numerical/math/MatrixExponential.hpp

@@ -0,0 +1,159 @@
+#pragma once
+#if defined(__GNUC__) || defined(__clang__)
+#pragma GCC optimize("O3", "fast-math")
+#endif
+#include "numerical/math/CompilerOptimizations.hpp"
+#include "numerical/math/Matrix.hpp"
+#include "numerical/math/MatrixNorms.hpp"
+#include "numerical/math/TriangularSolve.hpp"
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <type_traits>
+
+namespace math
+{
+    template<typename T, std::size_t N>
+    class MatrixExponential
+    {
+        static_assert(std::is_floating_point_v<T>, "MatrixExponential supports floating-point types");
+
+    public:
+        MatrixExponential() = default;
+
+        OPTIMIZE_FOR_SPEED SquareMatrix<T, N> Compute(const SquareMatrix<T, N>& a);
+        OPTIMIZE_FOR_SPEED SquareMatrix<T, N> Compute(const SquareMatrix<T, N>& a, T dt);
+
+    private:
+        static constexpr T c0{ T{ 1 } };
+        static constexpr T c1{ T{ 1 } / T{ 2 } };
+        static constexpr T c2{ T{ 5 } / T{ 44 } };
+        static constexpr T c3{ T{ 1 } / T{ 66 } };
+        static constexpr T c4{ T{ 1 } / T{ 792 } };
+        static constexpr T c5{ T{ 1 } / T{ 15840 } };
+        static constexpr T c6{ T{ 1 } / T{ 665280 } };
+
+        void PadeNumeratorDenominator(const SquareMatrix<T, N>& as,
+            SquareMatrix<T, N>& num,
+            SquareMatrix<T, N>& den);
+
+        SquareMatrix<T, N> SolvePade(const SquareMatrix<T, N>& den, const SquareMatrix<T, N>& num);
+    };
+
+    template<typename T, std::size_t N>
+    void MatrixExponential<T, N>::PadeNumeratorDenominator(const SquareMatrix<T, N>& as,
+        SquareMatrix<T, N>& num,
+        SquareMatrix<T, N>& den)
+    {
+        const auto identity = SquareMatrix<T, N>::Identity();
+        const auto a2 = as * as;
+        const auto a4 = a2 * a2;
+        const auto a6 = a4 * a2;
+
+        const auto vEven = identity * c0 + a2 * c2 + a4 * c4 + a6 * c6;
+        const auto uOdd = as * (identity * c1 + a2 * c3 + a4 * c5);
+
+        num = vEven + uOdd;
+        den = vEven - uOdd;
+    }
+
+    template<typename T, std::size_t N>
+    SquareMatrix<T, N> MatrixExponential<T, N>::SolvePade(const SquareMatrix<T, N>& den, const SquareMatrix<T, N>& num)
+    {
+        SquareMatrix<T, N> lu = den;
+        std::array<std::size_t, N> piv{};
+        for (std::size_t i = 0; i < N; ++i)
+            piv[i] = i;
+
+        for (std::size_t k = 0; k < N; ++k)
+        {
+            std::size_t p = k;
+            T maxVal = std::abs(lu.at(k, k));
+            for (std::size_t i = k + 1; i < N; ++i)
+            {
+                T candidate = std::abs(lu.at(i, k));
+                if (candidate > maxVal)
+                {
+                    maxVal = candidate;
+                    p = i;
+                }
+            }
+
+            if (p != k)
+            {
+                for (std::size_t j = 0; j < N; ++j)
+                {
+                    T tmp = lu.at(p, j);
+                    lu.at(p, j) = lu.at(k, j);
+                    lu.at(k, j) = tmp;
+                }
+                std::size_t tmpIdx = piv[p];
+                piv[p] = piv[k];
+                piv[k] = tmpIdx;
+            }
+
+            for (std::size_t i = k + 1; i < N; ++i)
+            {
+                lu.at(i, k) /= lu.at(k, k);
+                for (std::size_t j = k + 1; j < N; ++j)
+                    lu.at(i, j) -= lu.at(i, k) * lu.at(k, j);
+            }
+        }
+
+        SquareMatrix<T, N> result{};
+        for (std::size_t col = 0; col < N; ++col)
+        {
+            Vector<T, N> b{};
+            for (std::size_t i = 0; i < N; ++i)
+                b.at(i, 0) = num.at(piv[i], col);
+
+            Vector<T, N> y = SolveUnitLowerTriangular(lu, b);
+            Vector<T, N> x = SolveUpperTriangular(lu, y);
+
+            for (std::size_t i = 0; i < N; ++i)
+                result.at(i, col) = x.at(i, 0);
+        }
+
+        return result;
+    }
+
+    template<typename T, std::size_t N>
+    OPTIMIZE_FOR_SPEED SquareMatrix<T, N> MatrixExponential<T, N>::Compute(const SquareMatrix<T, N>& a)
+    {
+        const T norm = InfinityNorm(a);
+        int s{ 0 };
+        if (norm > T{ 1 })
+        {
+            const T lg = std::log2(norm);
+            s = static_cast<int>(std::ceil(lg));
+            if (s < 0)
+                s = 0;
+        }
+
+        const T scale = T{ 1 } / static_cast<T>(1 << s);
+        const auto as = a * scale;
+
+        SquareMatrix<T, N> num{};
+        SquareMatrix<T, N> den{};
+        PadeNumeratorDenominator(as, num, den);
+
+        auto r = SolvePade(den, num);
+
+        for (int k = 0; k < s; ++k)
+            r = r * r;
+
+        return r;
+    }
+
+    template<typename T, std::size_t N>
+    OPTIMIZE_FOR_SPEED SquareMatrix<T, N> MatrixExponential<T, N>::Compute(const SquareMatrix<T, N>& a, T dt)
+    {
+        const auto adt = a * dt;
+        return Compute(adt);
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class MatrixExponential<float, 2>;
+    extern template class MatrixExponential<float, 3>;
+#endif
+}

numerical/math/test/CMakeLists.txt

@@ -13,6 +13,7 @@ target_sources(numerical.math_test PRIVATE
     TestGivensRotation.cpp
     TestHouseholderTransform.cpp
     TestLinearTimeInvariant.cpp
+    TestMatrixExponential.cpp
     TestMatrixNorms.cpp
     TestMatrixOperations.cpp
     TestQNumber.cpp