Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 19 additions & 5 deletions doc/math/MatrixOperations.md
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,10 @@ Small structural matrix utilities that are needed across the library but do not
- **`Symmetrize`** enforces exact symmetry on a matrix that should be symmetric in theory but drifts under floating-point round-off.
- **`CongruenceTransform`** computes the quadratic form `A·M·Aᵀ` — the single most common shape in covariance-propagating code (Kalman predict `F·P·Fᵀ`, innovation covariance `H·P·Hᵀ`, Joseph update `(I−KH)·P·(I−KH)ᵀ`, etc.).

Both are recurring needs in Kalman filters, EM parameter updates, and Riccati/Lyapunov solvers.
- **`Diagonal`** and **`DiagonalOf`** convert between a vector and a diagonal matrix.
- **`ScaleColumns`** and **`DiagonalCongruence`** compute `M·diag(d)` and `diag(d)·M·diag(d)` without forming the diagonal matrix: per-joint gains, sign conventions and unit scalings applied to Jacobians and mass matrices.

The first two are recurring needs in Kalman filters, EM parameter updates, and Riccati/Lyapunov solvers; the diagonal products recur wherever a matrix is expressed in rescaled coordinates.

## Mathematical Theory

Expand All @@ -19,19 +22,30 @@ $$M = \underbrace{\tfrac{1}{2}(M + M^\top)}_{\text{symmetric}} + \underbrace{\tf

If $M$ is symmetric, `CongruenceTransform` returns a symmetric result exactly (in exact arithmetic): $(AMA^\top)^\top = A M^\top A^\top = A M A^\top$. This makes it the natural building block for propagating a covariance $P$ through a linear map $A$: $P \mapsto A P A^\top$.

With $D = \operatorname{diag}(d)$, a product with a diagonal matrix scales columns, or rows and columns, entry by entry:

$$(M D)_{ij} = M_{ij}\, d_j, \qquad (D M D)_{ij} = d_i\, M_{ij}\, d_j$$

so neither product needs a matrix multiplication. $D M D$ is a congruence: it keeps a symmetric $M$ symmetric, and a positive-definite $M$ positive definite when every $d_i \neq 0$.

## Complexity Analysis

| Operation | Time | Space | Notes |
|---------------------|--------------------|----------|---------------------------------------------------------|
| Symmetrize | $O(n^2)$ | $O(n^2)$ | One transpose, add, scale |
| CongruenceTransform | $O(n^2 m + n m^2)$ | $O(nm)$ | For $A \in \mathbb{R}^{n\times m}$, two matrix products |
| Operation | Time | Space | Notes |
|----------------------|--------------------|----------|-------------------------------------------------------------------------|
| Symmetrize | $O(n^2)$ | $O(n^2)$ | One transpose, add, scale |
| CongruenceTransform | $O(n^2 m + n m^2)$ | $O(nm)$ | For $A \in \mathbb{R}^{n\times m}$, two matrix products |
| Diagonal, DiagonalOf | $O(n)$ | $O(n^2)$ | Copies the diagonal only |
| ScaleColumns | $O(nm)$ | $O(nm)$ | One multiplication per entry |
| DiagonalCongruence | $O(n^2)$ | $O(n^2)$ | Two multiplications per entry, against $O(n^3)$ for two matrix products |

## Step-by-Step Walkthrough

**Symmetrize** — for $M = \begin{bmatrix}1 & 3 \\ -1 & 2\end{bmatrix}$: $M^\top = \begin{bmatrix}1 & -1 \\ 3 & 2\end{bmatrix}$, so $\tfrac{1}{2}(M + M^\top) = \begin{bmatrix}1 & 1 \\ 1 & 2\end{bmatrix}$ — off-diagonals averaged, diagonal unchanged.

**CongruenceTransform** — with $A \in \mathbb{R}^{n\times m}$ and symmetric $M \in \mathbb{R}^{m\times m}$, the result $A M A^\top \in \mathbb{R}^{n\times n}$ is the covariance of $A x$ when $x$ has covariance $M$.

**DiagonalCongruence** — for $d = (2, -3)$ and $M = \begin{bmatrix}1 & 3 \\ -1 & 2\end{bmatrix}$: $(DMD)_{01} = 2 \cdot 3 \cdot (-3) = -18$, $(DMD)_{10} = -3 \cdot (-1) \cdot 2 = 6$ and $(DMD)_{11} = 9 \cdot 2 = 18$, as $\operatorname{diag}(d)\,M\,\operatorname{diag}(d)$ gives with two full products.

## Pitfalls & Edge Cases

**Symmetrize is not a fix for indefiniteness** — it removes the skew part but does not make a matrix positive-definite; covariance code typically also adds a small diagonal jitter (`+ εI`) separately.
Expand Down
2 changes: 1 addition & 1 deletion doc/math/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ Core mathematical primitives for numerical computation.
| [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive |
| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR |
| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU |
| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) |
| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize`, `CongruenceTransform`, diagonal matrices and entry-wise diagonal products |
| [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 |
| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared |
Expand Down
42 changes: 42 additions & 0 deletions numerical/math/MatrixOperations.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,4 +20,46 @@ namespace math
static_assert(std::is_floating_point_v<T>, "CongruenceTransform supports floating-point types");
return a * m * a.Transpose();
}

template<typename T, std::size_t N>
[[nodiscard]] SquareMatrix<T, N> Diagonal(const Vector<T, N>& d)
{
static_assert(std::is_floating_point_v<T>, "Diagonal supports floating-point types");
SquareMatrix<T, N> result{};
for (std::size_t i = 0; i < N; ++i)
result.at(i, i) = d.at(i, 0);
return result;
}

template<typename T, std::size_t N>
[[nodiscard]] Vector<T, N> DiagonalOf(const SquareMatrix<T, N>& m)
{
static_assert(std::is_floating_point_v<T>, "DiagonalOf supports floating-point types");
Vector<T, N> result{};
for (std::size_t i = 0; i < N; ++i)
result.at(i, 0) = m.at(i, i);
return result;
}

template<typename T, std::size_t Rows, std::size_t Cols>
[[nodiscard]] OPTIMIZE_FOR_SPEED Matrix<T, Rows, Cols> ScaleColumns(const Matrix<T, Rows, Cols>& m, const Vector<T, Cols>& d)
{
static_assert(std::is_floating_point_v<T>, "ScaleColumns supports floating-point types");
Matrix<T, Rows, Cols> result{ m };
for (std::size_t i = 0; i < Rows; ++i)
for (std::size_t j = 0; j < Cols; ++j)
result.at(i, j) *= d.at(j, 0);
return result;
}

template<typename T, std::size_t N>
[[nodiscard]] OPTIMIZE_FOR_SPEED SquareMatrix<T, N> DiagonalCongruence(const Vector<T, N>& d, const SquareMatrix<T, N>& m)
{
static_assert(std::is_floating_point_v<T>, "DiagonalCongruence supports floating-point types");
SquareMatrix<T, N> result{ m };
for (std::size_t i = 0; i < N; ++i)
for (std::size_t j = 0; j < N; ++j)
result.at(i, j) *= d.at(i, 0) * d.at(j, 0);
return result;
}
}
39 changes: 39 additions & 0 deletions numerical/math/test/TestMatrixOperations.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -161,3 +161,42 @@ TEST_F(MatrixOperationsTest, CongruenceTransformZeroMYieldsZeroResult)
for (std::size_t j = 0; j < 2; ++j)
EXPECT_NEAR(result.at(i, j), 0.0f, math::Tolerance<float>());
}

TEST_F(MatrixOperationsTest, DiagonalPlacesTheVectorOnTheDiagonalAndDiagonalOfReadsItBack)
{
const math::Vector<float, 2> d{ 2.0f, -3.0f };

const auto matrix = math::Diagonal(d);
const auto back = math::DiagonalOf(matrix);

EXPECT_NEAR(matrix.at(0, 0), 2.0f, math::Tolerance<float>());
EXPECT_NEAR(matrix.at(1, 1), -3.0f, math::Tolerance<float>());
EXPECT_NEAR(matrix.at(0, 1), 0.0f, math::Tolerance<float>());
EXPECT_NEAR(matrix.at(1, 0), 0.0f, math::Tolerance<float>());
EXPECT_NEAR(back.at(0, 0), 2.0f, math::Tolerance<float>());
EXPECT_NEAR(back.at(1, 0), -3.0f, math::Tolerance<float>());
}

TEST_F(MatrixOperationsTest, ScaleColumnsMatchesTheProductWithADiagonalOnTheRight)
{
const math::Vector<float, 2> d{ 2.0f, -3.0f };

const auto result = math::ScaleColumns(asymmetric, d);
const auto expected = asymmetric * math::Diagonal(d);

for (std::size_t i = 0; i < 2; ++i)
for (std::size_t j = 0; j < 2; ++j)
EXPECT_NEAR(result.at(i, j), expected.at(i, j), math::Tolerance<float>());
}

TEST_F(MatrixOperationsTest, DiagonalCongruenceMatchesTheProductWithADiagonalOnBothSides)
{
const math::Vector<float, 2> d{ 2.0f, -3.0f };

const auto result = math::DiagonalCongruence(d, asymmetric);
const auto expected = math::Diagonal(d) * asymmetric * math::Diagonal(d);

for (std::size_t i = 0; i < 2; ++i)
for (std::size_t j = 0; j < 2; ++j)
EXPECT_NEAR(result.at(i, j), expected.at(i, j), math::Tolerance<float>());
}
Loading