chore: add matrix norms (#196)

* add matrix norms

* Apply suggestions from code review

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

* fix sonar findings

---------

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

Author: gabrielfrasantos

README.md

@@ -26,8 +26,8 @@ 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, Step Response Metrics                            |
-| [Solvers](doc/solvers/README.md)                                   | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince) |
+| [Math](doc/math/README.md)                                         | CORDIC, Quaternion, MatrixNorms, Step Response Metrics                                      |
+| [Solvers](doc/solvers/README.md)                                   | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Condition Number |
 | [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations, SIMD                                         |
 
 Each category page lists its algorithms with a brief description and links to the detailed documentation.

ROADMAP.md

@@ -48,7 +48,6 @@ Difficulty legend:
 | 45 | IIR filter design (Butterworth/Chebyshev + bilinear) | `filters/passive`         | ★★★★★      |
 | 46 | H∞ state-feedback control                            | `robust_control` (new)    | ★★★★★      |
 | 47 | Model Reference Adaptive Control (MRAC)              | `nonlinear_control` (new) | ★★★★★      |
-| 50 | Matrix norms & condition number                      | `math`                    | ★★★☆☆      |
 | 51 | Spectral radius / discrete stability margin          | `math`                    | ★★★☆☆      |
 | 52 | Estimator consistency metrics (NEES / NIS)           | `estimators`              | ★★★☆☆      |
 
@@ -559,16 +558,6 @@ and [`control_analysis/FrequencyResponse`](numerical/control_analysis/FrequencyR
 magnitude/phase. The items below are the missing pieces. All are **float-only**, no-heap, and operate
 on bounded `math::Vector`/`math::Matrix` inputs; tests are `TEST_F` on `float`.
 
-### 50. Matrix norms & condition number ★★★☆☆ — `math`
-- **What:** `FrobeniusNorm`, `OneNorm`, `InfinityNorm` on `Matrix`; `Vector` `Norm`/`Normalize`;
-  `ConditionNumber` estimate.
-- **Metric value:** M9 (conditioning) — quantifies ill-conditioning for `solvers/`, regression, and
-  Kalman covariance sanity; foundational gap ([`Matrix`](numerical/math/Matrix.hpp) currently exposes
-  only `Transpose`/`Trace`).
-- **Algorithm:** direct norm sums; condition number from norm ratio (apply the inverse via the
-  existing `GaussianElimination` rather than forming it explicitly, embedded-style).
-- **Reuses:** `math::Matrix`, `solvers::GaussianElimination`.
-
 ### 51. Spectral radius / discrete stability margin ★★★☆☆ — `math`
 - **What:** Dominant `|eigenvalue|` of a square (state/companion) matrix; `IsSchurStable` (all
   `|λ| < 1`) and the stability margin `1 − ρ(A)`.

doc/math/MatrixNorms.md

@@ -0,0 +1,86 @@
+# Matrix & Vector Norms
+
+## Overview & Motivation
+
+Linear algebra computations — solvers, Kalman filter covariance updates, regression — depend on the numerical health of the matrices involved. Norms formalise the notion of "size" for matrices and vectors and are the building blocks of every conditioning, stability, and error-bound estimate in the library. They are also the cheapest such quantities to compute: a single pass over the entries with no allocation, suitable for real-time embedded paths.
+
+Vector normalisation, closely related, produces the unit-length direction of a vector and is a recurring primitive in geometry, attitude estimation, and gradient methods.
+
+## Mathematical Theory
+
+### Vector Norm
+
+For a vector $\mathbf{v} \in \mathbb{R}^n$, the Euclidean (L2) norm is
+
+$$\|\mathbf{v}\|_2 = \sqrt{\sum_{i=1}^{n} v_i^2}$$
+
+The unit vector $\hat{\mathbf{v}} = \mathbf{v} / \|\mathbf{v}\|_2$ satisfies $\|\hat{\mathbf{v}}\|_2 = 1$. Normalisation is undefined when $\|\mathbf{v}\|_2 = 0$ and must be guarded.
+
+### Matrix Norms
+
+**Frobenius norm** — treats the matrix as a flattened vector:
+
+$$\|A\|_F = \sqrt{\sum_{i=1}^{m}\sum_{j=1}^{n} a_{ij}^2}$$
+
+It is rotationally invariant under unitary transformations and cheap to compute.
+
+**1-norm (maximum absolute column sum)**:
+
+$$\|A\|_1 = \max_{1 \le j \le n} \sum_{i=1}^{m} |a_{ij}|$$
+
+**Infinity norm (maximum absolute row sum)**:
+
+$$\|A\|_\infty = \max_{1 \le i \le m} \sum_{j=1}^{n} |a_{ij}|$$
+
+The 1-norm and infinity-norm are dual: $\|A\|_\infty = \|A^\top\|_1$.
+
+## Complexity Analysis
+
+| Operation     | Time    | Space  | Notes                        |
+|---------------|---------|--------|------------------------------|
+| FrobeniusNorm | $O(mn)$ | $O(1)$ | Single pass, no allocation   |
+| OneNorm       | $O(mn)$ | $O(1)$ | Column-wise sum, running max |
+| InfinityNorm  | $O(mn)$ | $O(1)$ | Row-wise sum, running max    |
+| VectorNorm    | $O(n)$  | $O(1)$ | Single pass                  |
+| Normalize     | $O(n)$  | $O(n)$ | Output vector on stack       |
+
+## Step-by-Step Walkthrough
+
+Matrix $A = \begin{bmatrix}3 & 1 \\ 1 & 2\end{bmatrix}$:
+
+1. **FrobeniusNorm**: $\sqrt{9 + 1 + 1 + 4} = \sqrt{15} \approx 3.873$
+2. **OneNorm**: column 0 sum $= |3| + |1| = 4$; column 1 sum $= |1| + |2| = 3$; max $= 4$
+3. **InfinityNorm**: row 0 sum $= |3| + |1| = 4$; row 1 sum $= |1| + |2| = 3$; max $= 4$
+
+Vector $\mathbf{v} = [3,\, 4]^\top$: $\|\mathbf{v}\|_2 = 5$, and $\hat{\mathbf{v}} = [0.6,\, 0.8]^\top$.
+
+## Pitfalls & Edge Cases
+
+**Zero vector normalisation** — dividing by $\|\mathbf{v}\|_2 = 0$ is undefined. The implementation returns an empty optional for near-zero norms.
+
+**Fast-math semantics** — `#pragma GCC optimize("fast-math")` may reorder floating-point operations. The norms are sums of non-negative values, so reordering does not change the sign of the result, but catastrophic cancellation can still occur for near-zero off-diagonal entries.
+
+**Non-square matrices** — FrobeniusNorm, OneNorm, and InfinityNorm apply to any $m \times n$ matrix.
+
+## Variants & Generalizations
+
+The **spectral norm** (largest singular value, $\|A\|_2$) is the tightest but requires an SVD — $O(N^3)$ with a large constant, unsuitable for embedded real-time paths. The 1-norm and infinity-norm are cheap upper bounds used throughout the library instead.
+
+General **p-norms** and weighted norms generalise the vector case; the L2 norm is the only one currently exposed because it is the natural quantity for geometric and least-squares work.
+
+## Applications
+
+- **Conditioning estimates**: the 1-norm feeds the condition number (see `solvers::ConditionNumber`).
+- **Convergence tests**: iterative solvers and optimisers stop when a residual norm falls below tolerance.
+- **Attitude / geometry**: vector normalisation produces unit direction and rotation axes.
+- **Covariance sanity**: the Frobenius norm of a covariance matrix bounds its total variance.
+
+## Connections to Other Algorithms
+
+The 1-norm is the norm used by `solvers::ConditionNumber` for its $\|A\|\cdot\|A^{-1}\|$ estimate. The `math::Matrix` type provides the storage and transpose the norms operate on. Norm-based residual tests appear in `solvers` and `optimization`.
+
+## References & Further Reading
+
+- Golub, G. H. & Van Loan, C. F., "Matrix Computations", 4th ed., Chapter 2 (matrix norms)
+- Trefethen, L. N. & Bau, D., "Numerical Linear Algebra", Lecture 3 (norms)
+- Higham, N. J., "Accuracy and Stability of Numerical Algorithms", 2nd ed.

doc/math/README.md

@@ -8,4 +8,5 @@ Core mathematical primitives for numerical computation.
 |-----------------------------|-----------------------------------------------------------------------------------------------|
 | [CORDIC](Cordic.md)                                 | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier |
 | [Quaternion](Quaternion.md)                         | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions |
+| [MatrixNorms](MatrixNorms.md)                       | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize                        |
 | [Step Response Metrics](StepResponseMetrics.md)     | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector |

doc/solvers/ConditionNumber.md

@@ -0,0 +1,68 @@
+# Condition Number
+
+## Overview & Motivation
+
+A matrix can be mathematically invertible yet catastrophically sensitive to perturbations in its entries or in the right-hand side of a linear system. This sensitivity, called ill-conditioning, is the root cause of accumulated floating-point error in solvers, regression, and Kalman-filter covariance updates. Measuring it before committing to a solve reveals whether the result can be trusted or whether regularisation, pivoting, or reformulation is warranted.
+
+The condition number is the canonical scalar summary of ill-conditioning. Because it requires the norm of the inverse, it depends on a linear solver — which is why it lives in the `solvers` layer, on top of the `math` norms, rather than in `math` itself.
+
+## Mathematical Theory
+
+For an invertible square matrix $A \in \mathbb{R}^{N \times N}$ and a chosen matrix norm $\|\cdot\|$:
+
+$$\kappa(A) = \|A\| \cdot \|A^{-1}\|$$
+
+The condition number bounds the relative error amplification in the solution $\mathbf{x}$ of $A\mathbf{x} = \mathbf{b}$ due to a perturbation $\delta\mathbf{b}$:
+
+$$\frac{\|\delta\mathbf{x}\|}{\|\mathbf{x}\|} \le \kappa(A) \cdot \frac{\|\delta\mathbf{b}\|}{\|\mathbf{b}\|}$$
+
+$\kappa(A) \ge 1$ always; $\kappa(A) = 1$ only for scalar multiples of orthogonal matrices. A singular matrix has $\kappa(A) = \infty$. The estimate here uses the 1-norm.
+
+### Computing the Inverse via Column-wise Solve
+
+The norm of $A^{-1}$ is obtained without forming an explicit inverse as a first-class object: solving $AX = I$ column by column with partial-pivoting Gaussian elimination yields the inverse columns, from which the 1-norm is accumulated. This mirrors the embedded convention of reusing the existing solver rather than adding a dedicated inversion routine.
+
+## Complexity Analysis
+
+| Operation       | Time     | Space    | Notes                                         |
+|-----------------|----------|----------|-----------------------------------------------|
+| ConditionNumber | $O(N^3)$ | $O(N^2)$ | Dominated by the $N$-column solve of $AX = I$ |
+
+## Step-by-Step Walkthrough
+
+Matrix $A = \begin{bmatrix}3 & 1 \\ 1 & 2\end{bmatrix}$, 1-norm condition number:
+
+1. $\|A\|_1 = \max(4, 3) = 4$
+2. Solve $A\mathbf{x} = \mathbf{e}_0$: $\mathbf{x}_0 = [0.4,\, -0.2]^\top$
+3. Solve $A\mathbf{x} = \mathbf{e}_1$: $\mathbf{x}_1 = [-0.2,\, 0.6]^\top$
+4. $A^{-1} = \begin{bmatrix}0.4 & -0.2 \\ -0.2 & 0.6\end{bmatrix}$, so $\|A^{-1}\|_1 = \max(0.6, 0.8) = 0.8$
+5. $\kappa_1(A) = 4 \times 0.8 = 3.2$
+
+## Pitfalls & Edge Cases
+
+**Singular matrices** — a row of all zeros makes the system unsolvable. The singularity check fires before entering Gaussian elimination, returning an empty result rather than dividing by zero.
+
+**Near-singular matrices** — the condition number can be astronomically large without any row being identically zero. Partial-pivoting elimination still completes, and the returned value reflects the ill-conditioning faithfully.
+
+**Norm choice** — the 1-norm condition number is a practical upper bound on the spectral condition number $\kappa_2$; it is cheaper by orders of magnitude because it avoids the SVD.
+
+## Variants & Generalizations
+
+The **spectral condition number** ($\kappa_2$, ratio of largest to smallest singular value) is the tightest but requires an SVD. The **reciprocal condition number** ($\text{RCOND} = 1/\kappa$) avoids overflow when $\kappa$ is very large; LAPACK-style solvers return this form and compare against machine epsilon. For **positive-definite** systems, Cholesky factorisation supplies the same column-wise solves at half the cost.
+
+## Applications
+
+- **Solver validation**: if $\kappa(A)\cdot\epsilon_{\text{mach}} \gtrsim 1$, the solution has no reliable digits.
+- **Kalman filter covariance**: monitoring $\kappa(P)$ detects numerical collapse of the covariance.
+- **Regression**: the design-matrix condition number governs least-squares sensitivity to noise.
+- **Control design**: ill-conditioned system matrices signal near-uncontrollability or near-unobservability.
+
+## Connections to Other Algorithms
+
+Gaussian elimination with partial pivoting (`solvers::GaussianElimination`) supplies the column-wise solves of $AX = I$. The matrix 1-norm (`math::OneNorm`) supplies both factors of the product. Cholesky decomposition (`solvers::CholeskyDecomposition`) is the positive-definite alternative for the inverse solve.
+
+## References & Further Reading
+
+- Golub, G. H. & Van Loan, C. F., "Matrix Computations", 4th ed., Chapter 3 (Gaussian elimination)
+- Higham, N. J., "Accuracy and Stability of Numerical Algorithms", 2nd ed., Chapter 6 (condition numbers)
+- Trefethen, L. N. & Bau, D., "Numerical Linear Algebra", Lecture 12 (conditioning)

doc/solvers/README.md

@@ -12,3 +12,4 @@ Numerical solvers for linear systems, polynomial roots, and matrix equations.
 | [Durand-Kerner](DurandKerner.md)                                           | Simultaneous iterative root-finder for polynomials                     |
 | [Discrete Algebraic Riccati Equation](DiscreteAlgebraicRiccatiEquation.md) | Iterative solver for the DARE arising in LQR and Kalman filter design  |
 | [Runge-Kutta ODE Integrators](RungeKuttaIntegrators.md)                    | Fixed-step RK4 and adaptive Dormand-Prince RK45 for ODE integration    |
+| [Condition Number](ConditionNumber.md)                                     | 1-norm condition number estimate via column-wise inverse solve         |

numerical/math/CMakeLists.txt

@@ -17,6 +17,7 @@ target_sources(numerical.math PRIVATE
     HyperbolicFunctions.hpp
     LinearTimeInvariant.hpp
     Matrix.hpp
+    MatrixNorms.hpp
     QNumber.hpp
     Quaternion.hpp
     RecursiveBuffer.hpp

numerical/math/MatrixNorms.hpp

@@ -0,0 +1,79 @@
+#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 <cmath>
+#include <cstddef>
+#include <optional>
+#include <type_traits>
+
+namespace math
+{
+    template<typename T, std::size_t Rows, std::size_t Cols>
+    [[nodiscard]] OPTIMIZE_FOR_SPEED T FrobeniusNorm(const Matrix<T, Rows, Cols>& a)
+    {
+        static_assert(std::is_floating_point_v<T>, "MatrixNorms supports floating-point types");
+        T sum{};
+        for (std::size_t i = 0; i < Rows; ++i)
+            for (std::size_t j = 0; j < Cols; ++j)
+                sum += a.at(i, j) * a.at(i, j);
+        return std::sqrt(sum);
+    }
+
+    template<typename T, std::size_t Rows, std::size_t Cols>
+    [[nodiscard]] OPTIMIZE_FOR_SPEED T OneNorm(const Matrix<T, Rows, Cols>& a)
+    {
+        static_assert(std::is_floating_point_v<T>, "MatrixNorms supports floating-point types");
+        T maxColSum{};
+        for (std::size_t j = 0; j < Cols; ++j)
+        {
+            T colSum{};
+            for (std::size_t i = 0; i < Rows; ++i)
+                colSum += std::abs(a.at(i, j));
+            if (colSum > maxColSum)
+                maxColSum = colSum;
+        }
+        return maxColSum;
+    }
+
+    template<typename T, std::size_t Rows, std::size_t Cols>
+    [[nodiscard]] OPTIMIZE_FOR_SPEED T InfinityNorm(const Matrix<T, Rows, Cols>& a)
+    {
+        static_assert(std::is_floating_point_v<T>, "MatrixNorms supports floating-point types");
+        T maxRowSum{};
+        for (std::size_t i = 0; i < Rows; ++i)
+        {
+            T rowSum{};
+            for (std::size_t j = 0; j < Cols; ++j)
+                rowSum += std::abs(a.at(i, j));
+            if (rowSum > maxRowSum)
+                maxRowSum = rowSum;
+        }
+        return maxRowSum;
+    }
+
+    template<typename T, std::size_t Size>
+    [[nodiscard]] OPTIMIZE_FOR_SPEED T VectorNorm(const Vector<T, Size>& v)
+    {
+        static_assert(std::is_floating_point_v<T>, "MatrixNorms supports floating-point types");
+        T sum{};
+        for (std::size_t i = 0; i < Size; ++i)
+            sum += v.at(i, 0) * v.at(i, 0);
+        return std::sqrt(sum);
+    }
+
+    template<typename T, std::size_t Size>
+    [[nodiscard]] OPTIMIZE_FOR_SPEED std::optional<Vector<T, Size>> Normalize(const Vector<T, Size>& v)
+    {
+        static_assert(std::is_floating_point_v<T>, "MatrixNorms supports floating-point types");
+        T n = VectorNorm(v);
+        if (n < static_cast<T>(1e-12))
+            return std::nullopt;
+        Vector<T, Size> result;
+        for (std::size_t i = 0; i < Size; ++i)
+            result.at(i, 0) = v.at(i, 0) / n;
+        return result;
+    }
+}

numerical/math/test/CMakeLists.txt

@@ -11,6 +11,7 @@ target_sources(numerical.math_test PRIVATE
     TestComplexNumber.cpp
     TestCordic.cpp
     TestLinearTimeInvariant.cpp
+    TestMatrixNorms.cpp
     TestQNumber.cpp
     TestMatrix.cpp
     TestQuaternion.cpp

numerical/math/test/TestMatrixNorms.cpp

@@ -0,0 +1,55 @@
+#include "numerical/math/MatrixNorms.hpp"
+#include "numerical/math/Tolerance.hpp"
+#include <gtest/gtest.h>
+
+namespace
+{
+    class MatrixNormsTest : public ::testing::Test
+    {
+    protected:
+        math::Matrix<float, 2, 2> a{
+            { 3.0f, 1.0f },
+            { 1.0f, 2.0f }
+        };
+        math::Vector<float, 2> v{ { 3.0f }, { 4.0f } };
+    };
+}
+
+TEST_F(MatrixNormsTest, FrobeniusNorm)
+{
+    float result = math::FrobeniusNorm(a);
+    EXPECT_NEAR(result, 3.87298f, math::Tolerance<float>());
+}
+
+TEST_F(MatrixNormsTest, OneNorm)
+{
+    float result = math::OneNorm(a);
+    EXPECT_NEAR(result, 4.0f, math::Tolerance<float>());
+}
+
+TEST_F(MatrixNormsTest, InfinityNorm)
+{
+    float result = math::InfinityNorm(a);
+    EXPECT_NEAR(result, 4.0f, math::Tolerance<float>());
+}
+
+TEST_F(MatrixNormsTest, VectorNorm)
+{
+    float result = math::VectorNorm(v);
+    EXPECT_NEAR(result, 5.0f, math::Tolerance<float>());
+}
+
+TEST_F(MatrixNormsTest, NormalizeUnit)
+{
+    auto result = math::Normalize(v);
+    ASSERT_TRUE(result.has_value());
+    EXPECT_NEAR(result->at(0, 0), 0.6f, math::Tolerance<float>());
+    EXPECT_NEAR(result->at(1, 0), 0.8f, math::Tolerance<float>());
+}
+
+TEST_F(MatrixNormsTest, NormalizeZeroVectorReturnsNullopt)
+{
+    math::Vector<float, 2> zero{ { 0.0f }, { 0.0f } };
+    auto result = math::Normalize(zero);
+    EXPECT_FALSE(result.has_value());
+}