Fixes for control systems AND SVD and QR decomposition (#9)

Reviewed-on: #9 Co-authored-by: Cynopolis <quinn.henthorne@gmail.com>
2026-08-26 13:08:21 -04:00
parent 48b016d8b7
commit f0035ab67f
16 changed files with 6471 additions and 67 deletions
@@ -0,0 +1,411 @@
+#pragma once
+#include "Matrix.hpp"
+
+/**
+ * @brief library that uses Matrix.hpp and performs SVD on a matrix
+ *
+ * @note Fully templated: SVD works for ANY Matrix<R, C> with R, C in
+ *       1..255 (the uint8_t range of Matrix). There is no 5×5 limit.
+ *
+ * @note EMBEDDED CONSTRAINT — no heap. All working storage is stack
+ *       allocated as templated Matrix<N,N> buffers where
+ *       N = max(R, C). Peak stack usage per SVD call is
+ *       ≈ 11·N² floats (≈ 44·N² bytes):
+ *         N = 5   → ~1.1 KB
+ *         N = 10  → ~4.4 KB
+ *         N = 20  → ~18 KB
+ *         N = 50  → ~110 KB
+ *         N = 100 → ~440 KB
+ *         N = 255 → ~2.9 MB
+ *       Instantiate only the sizes that fit your call-stack budget.
+ */
+namespace SVD {
+/**
+ * @brief Compute the Singular Value Decomposition (SVD) of this matrix.
+ *
+ * Decomposes A into U × Σ × Vᵀ where:
+ *   - U is an m×k orthogonal matrix (left singular vectors)
+ *   - Σ is a k×k diagonal matrix with non-negative singular values
+ *     (stored as a k×1 column vector)
+ *   - Vᵀ is a k×n orthogonal matrix (right singular vectors, transposed)
+ *   - k = min(m, n)
+ *
+ * The decomposition satisfies: A ≈ U × diag(Σ) × Vᵀ
+ * Singular values are returned in descending order.
+ *
+ * Output storage conventions:
+ *   - U: Matrix<rows, columns> — first k columns are meaningful
+ *     (rows k..columns−1 are zero in the wide case)
+ *   - sigma: Matrix<columns, 1> — first k entries are the singular
+ *     values; entries beyond k (wide matrices only) are zero
+ *   - Vt: Matrix<columns, columns> — first k rows are meaningful
+ *     (zero-padded in the tall case)
+ *
+ * For wide matrices (rows < columns) the SVD is computed on Aᵀ and the
+ * factors are swapped back.
+ *
+ * @tparam rows Number of rows in A (1..255)
+ * @tparam columns Number of columns in A (1..255)
+ * @param matrixToDecompose Input: the matrix A
+ * @param U Output: left singular vectors (rows×columns matrix)
+ * @param sigma Output: singular values (columns×1 vector, sorted descending)
+ * @param Vt Output: right singular vectors transposed (columns×columns)
+ *
+ * @note This implementation uses Householder bidiagonalization followed
+ *       by block reduction: 2×2 blocks via closed form, larger blocks
+ *       via cyclic Jacobi eigen-decomposition of BᵀB with residual
+ *       singular values σᵢ = ‖B·vᵢ‖ (see docs/svd-refactor.md).
+ */
+template <uint8_t rows, uint8_t columns>
+void SVD(Matrix<rows, columns> &matrixToDecompose, Matrix<rows, columns> &U,
+         Matrix<columns, 1> &sigma, Matrix<columns, columns> &Vt);
+
+// ========================================================================
+// SVD Building Block Functions (for unit testing)
+//
+// Templated on the working-buffer size N. All block operations work on
+// N×N matrices with runtime bounds (m, n, p, blockSize, ...) — the
+// regions beyond the bounds are zero-padded working space.
+//
+// N is deduced from the Matrix arguments at the call site, e.g.
+//   Matrix<8, 8> W, QL, QR;
+//   SVD::Bidiagonalize(W, 6, 8, 6, QL, QR);   // N = 8 deduced
+// ========================================================================
+
+/**
+ * @brief Compute a Householder reflector vector.
+ *
+ * Given input vector x, computes normalized v and scalar alpha such that:
+ *   (I - 2·v·vᵀ) · x = [alpha, 0, 0, ...]ᵀ
+ *
+ * @param x Input vector (up to len elements)
+ * @param len Number of valid elements in x
+ * @param v Output: normalized Householder vector (length ≥ len)
+ * @param alpha Output: the resulting first element after reflection
+ * @return The norm of the input vector x
+ */
+static float ComputeHouseholder(const float *x, uint8_t len, float *v,
+                                float &alpha);
+
+/**
+ * @brief Apply a Householder reflection from the left.
+ *
+ * Transforms W = (I - 2·v·vᵀ) · W where v operates on rows [startRow..endRow]
+ * and is applied across all N columns (zero-padded columns are a no-op).
+ *
+ * @tparam N Working buffer size
+ * @param W Input/output: matrix to transform
+ * @param v Householder vector (length = endRow - startRow + 1)
+ * @param startRow First row index
+ * @param endRow Last row index
+ */
+template <uint8_t N>
+static void ApplyHouseholderLeft(Matrix<N, N> &W, const float *v,
+                                 uint8_t startRow, uint8_t endRow);
+
+/**
+ * @brief Apply a Householder reflection from the right.
+ *
+ * Transforms W = W · (I - 2·v·vᵀ) where v operates on columns
+ * [startCol..endCol] and is applied across all N rows (zero-padded rows
+ * are a no-op).
+ *
+ * @tparam N Working buffer size
+ * @param W Input/output: matrix to transform
+ * @param v Householder vector (length = endCol - startCol + 1)
+ * @param startCol First column index
+ * @param endCol Last column index
+ */
+template <uint8_t N>
+static void ApplyHouseholderRight(Matrix<N, N> &W, const float *v,
+                                  uint8_t startCol, uint8_t endCol);
+
+/**
+ * @brief Reduce a matrix to upper bidiagonal form using Householder reflections.
+ *
+ * Applies a sequence of Householder reflections to reduce the input matrix
+ * W (m×q, where q ≥ p, stored in N×N working space) to upper bidiagonal
+ * form B (p×q), accumulating the left and right transformation matrices
+ * in QL and QR respectively.
+ *
+ * Algorithm (Golub-Kahan bidiagonalization):
+ *   For k = 0 to p-1:
+ *     1. Left HH on column k, rows k..m-1: zero out subdiagonal below B[k+1][k]
+ *     2. Right HH on row k, cols k+2..q-1: zero out superdiagonal above B[k][k+1]
+ *
+ * The accumulated transformations satisfy:
+ *   QLᵀ · W_original · QR = B (upper bidiagonal)
+ *
+ * @tparam N Working buffer size (≥ m and ≥ q)
+ * @param W Input/output: matrix to bidiagonalize (first m×q used)
+ * @param m Number of rows in the working matrix
+ * @param q Number of columns in the working matrix (q ≥ p)
+ * @param p Rank = min(m, original_columns) — number of bidiagonalization steps
+ * @param QL Input/output: left Householder accumulation (initialized to identity)
+ * @param QR Input/output: right Householder accumulation (initialized to identity)
+ */
+template <uint8_t N>
+static void Bidiagonalize(Matrix<N, N> &W, uint8_t m, uint8_t q, uint8_t p,
+                          Matrix<N, N> &QL, Matrix<N, N> &QR);
+
+/**
+ * @brief Deflate a bidiagonal matrix by zeroing negligible superdiagonals.
+ *
+ * Scans the p×p upper-bidiagonal matrix stored in W and zeros out any
+ * superdiagonal element W[i][i+1] whose magnitude is negligible relative
+ * to the local diagonal scale (|W[i][i]| + |W[i+1][i+1]|). Deflating
+ * splits the matrix into independent unreduced blocks that can each be
+ * solved separately.
+ *
+ * @tparam N Working buffer size
+ * @param W Input/output: bidiagonal matrix (first p×p used)
+ * @param p Size of the bidiagonal matrix (min(rows, columns))
+ * @param tol Relative deflation tolerance (e.g. 1e-8f)
+ */
+template <uint8_t N>
+static void DeflateBidiagonal(Matrix<N, N> &W, uint8_t p, float tol);
+
+/**
+ * @brief Check whether a bidiagonal matrix has fully reduced to diagonal.
+ *
+ * Returns true when every superdiagonal element of the p×p bidiagonal
+ * matrix in W is (numerically) zero, i.e. the diagonal entries are the
+ * (unsorted) singular values and no unreduced blocks remain.
+ *
+ * @tparam N Working buffer size
+ * @param W Input: bidiagonal matrix (first p×p used)
+ * @param p Size of the bidiagonal matrix (min(rows, columns))
+ * @param tol Numerical zero threshold multiplier
+ * @return true when all superdiagonal elements are ~0
+ */
+template <uint8_t N>
+static bool BidiagonalIsDiagonal(const Matrix<N, N> &W, uint8_t p, float tol);
+
+/**
+ * @brief Compute the full SVD of a 2×2 upper-bidiagonal block (pure).
+ *
+ * Decomposes B = [[a, b], [0, d]] as:
+ *   B = Ublock · diag(sigma[0], sigma[1]) · Vblockᵀ
+ *
+ * Guarantees:
+ *   - sigma[0] ≥ sigma[1] ≥ 0 (singular values, from eigenvalues of BᵀB)
+ *   - Ublock and Vblock are orthogonal (columns are the left/right
+ *     singular vectors respectively; Vblock = scipy's Vᵀᵀ)
+ *   - Ublock · diag(sigma) · Vblockᵀ == B (within float tolerance)
+ *
+ * Math: eigenvectors of BᵀB = [[a², ab], [ab, b²+d²]] give the right
+ * singular vectors (v1 = normalize(ab, σ1²−a²) with a safe fallback when
+ * that vector is ~0; v2 = (−v1y, v1x)); left singular vectors are
+ * uᵢ = B·vᵢ/σᵢ with a rank-deficiency guard: when σᵢ ≈ 0 (i.e. ~1e-30),
+ * that U column is filled with the signed orthogonal complement of the
+ * other U column instead of dividing by ~0.
+ *
+ * @param a B[0][0] (first diagonal element)
+ * @param b B[0][1] (superdiagonal element)
+ * @param d B[1][1] (second diagonal element)
+ * @param Ublock Output: 2×2 left singular vectors (columns)
+ * @param Vblock Output: 2×2 right singular vectors (columns)
+ * @param sigma Output: singular values, sigma[0] ≥ sigma[1] ≥ 0
+ */
+static void SolveBidiagonalBlock2x2(float a, float b, float d,
+                                    float Ublock[2][2], float Vblock[2][2],
+                                    float sigma[2]);
+
+/**
+ * @brief Cyclic Jacobi eigenvalue algorithm for a symmetric matrix (pure).
+ *
+ * Reduces symmetric n×n matrix T to (near-)diagonal form IN PLACE using
+ * cyclic Jacobi rotations, accumulating the eigenvectors in V.
+ *
+ * On return:
+ *   - T's diagonal entries are the eigenvalues (off-diagonals ~0)
+ *   - evals[i] = T[i][i], UNSORTED, SIGNED (this is a general symmetric
+ *     eigen solver, not just for PSD matrices like T = BᵀB)
+ *   - columns of V are the corresponding eigenvectors (T·V = V·Λ)
+ *
+ * Convergence: relative off-diagonal tolerance 1e-10, hard-capped at
+ * 100 sweeps.
+ *
+ * @tparam N Working buffer size (≥ n)
+ * @param T Input/output: symmetric matrix (first n×n used, destroyed in place)
+ * @param n Matrix size
+ * @param evals Output: eigenvalues, unsorted, length ≥ n
+ * @param V Output: eigenvector matrix (first n×n used), columns are eigenvectors
+ */
+template <uint8_t N>
+static void JacobiEigenSymmetric(Matrix<N, N> &T, uint8_t n, float *evals,
+                                 Matrix<N, N> &V);
+
+/**
+ * @brief Fold a block SVD's factors into the QL/QR accumulators.
+ *
+ * Given the block SVD of a bidiagonal block, B = Ublock·Σ·Vblockᵀ, the
+ * accumulated Householder matrices must absorb the block factors:
+ *   QL[:, blockStart..blockStart+blockSize−1] ← QL[:, ...] · Ublock
+ *     (rows 0..rowsQL−1)
+ *   QR[:, blockStart..blockStart+blockSize−1] ← QR[:, ...] · Vblock
+ *     (rows 0..rowsQR−1)
+ *
+ * rowsQL / rowsQR are the meaningful row extents of the accumulators
+ * (e.g. for a wide matrix W = Aᵀ, QL carries n = rows(W) meaningful
+ * rows while QR is read back over its first m rows).
+ *
+ * In-place update is done through temporary buffers (updating QL's block
+ * columns while still reading them corrupts the result).
+ *
+ * @tparam N Working buffer size
+ * @param blockStart First column/row index of the block in W
+ * @param blockSize Size of the block (2, or > 2 for the Jacobi path)
+ * @param Ublock Left singular-vector factor of the block (first blockSize×blockSize used)
+ * @param Vblock Right singular-vector factor of the block (first blockSize×blockSize used)
+ * @param rowsQL Number of meaningful rows of QL
+ * @param rowsQR Number of meaningful rows of QR
+ * @param QL Input/output: left transformation accumulator
+ * @param QR Input/output: right transformation accumulator
+ */
+template <uint8_t N>
+static void ApplyBlockFactorsToAccumulators(uint8_t blockStart,
+                                            uint8_t blockSize,
+                                            const Matrix<N, N> &Ublock,
+                                            const Matrix<N, N> &Vblock,
+                                            uint8_t rowsQL, uint8_t rowsQR,
+                                            Matrix<N, N> &QL,
+                                            Matrix<N, N> &QR);
+
+/**
+ * @brief Solve a bidiagonal block larger than 2×2 via Jacobi eigen of BᵀB.
+ *
+ * Computes the full SVD of the unreduced upper-bidiagonal block
+ * W[blockStart..blockStart+blockSize−1] via eigendecomposition of the
+ * tridiagonal T = BᵀB:
+ *   1. Snapshot the ORIGINAL block diagonal/superdiagonal from W
+ *   2. Form T = BᵀB (tridiagonal symmetric)
+ *   3. JacobiEigenSymmetric on T → eigenvalues (unsorted) + V
+ *   4. Sort eigenvalues descending, reordering V columns
+ *   5. Compute RESIDUAL singular values: σᵢ = ‖B_orig · vᵢ‖
+ *      (NOT sqrt(eigenvalue) — forming BᵀB squares the condition number,
+ *       causing float noise to swamp true tiny eigenvalues for
+ *       rank-deficient blocks)
+ *   6. Re-sort σ descending, keeping V and B·v consistent
+ *   7. Build Ublock: uᵢ = B_orig · vᵢ / σᵢ (unit norm); for σᵢ ≈ 0,
+ *      use Gram-Schmidt orthogonal completion against prior U columns
+ *   8. Fold Ublock/Vblock into QL/QR via ApplyBlockFactorsToAccumulators
+ *   9. Write residual norms into W's diagonal and zero the block's
+ *      superdiagonals
+ *
+ * @tparam N Working buffer size (≥ blockSize)
+ * @param W Input/output: bidiagonal matrix; the block's diagonal holds
+ *        the singular values and its superdiagonals are zeroed on return
+ * @param blockStart First column/row index of the block
+ * @param blockSize Size of the block (> 2)
+ * @param rowsQL Number of meaningful rows of QL
+ * @param rowsQR Number of meaningful rows of QR
+ * @param QL Input/output: left transformation accumulator
+ * @param QR Input/output: right transformation accumulator
+ */
+template <uint8_t N>
+static void SolveBidiagonalBlockJacobi(Matrix<N, N> &W, uint8_t blockStart,
+                                       uint8_t blockSize, uint8_t rowsQL,
+                                       uint8_t rowsQR, Matrix<N, N> &QL,
+                                       Matrix<N, N> &QR);
+
+/**
+ * @brief Extract singular values from bidiagonal matrix diagonal and sort.
+ *
+ * Extracts absolute values of diagonal elements of W as singular values,
+ * then sorts them in descending order while reordering columns of QL
+ * and QR to maintain consistency. A negative diagonal element flips the
+ * sign of the corresponding QL column to keep A = U·Σ·Vᵀ.
+ *
+ * @tparam N Working buffer size
+ * @param W Input: bidiagonal matrix (first p×p used)
+ * @param sigma Output: sorted singular values (N×1 column vector, only first p used)
+ * @param p Number of singular values (min(rows, columns))
+ * @param QL Input/output: left transformation matrix (modified during sort)
+ * @param QR Input/output: right transformation matrix (modified during sort)
+ */
+template <uint8_t N>
+static void ExtractAndSortSingularValues(Matrix<N, N> &W, Matrix<N, 1> &sigma,
+                                         uint8_t p, Matrix<N, N> &QL,
+                                         Matrix<N, N> &QR);
+
+/**
+ * @brief Assemble final U and Vt matrices from QL/QR.
+ *
+ * Computes the final left singular vectors (U) and right singular vectors
+ * transposed (Vt) from the accumulated Householder transformations.
+ *
+ * For non-transpose case: U = QL[:,0:p], Vt = QR[:,0:p]ᵀ
+ * For transpose case:    U = QR[:,0:p], Vt = full QLᵀ (all n rows)
+ *
+ * @tparam N Working buffer size (≥ m and ≥ n)
+ * @param m Number of rows in original matrix
+ * @param n Number of columns in original matrix
+ * @param p Rank = min(m, n)
+ * @param transposeNeeded True if we computed SVD(Aᵀ) instead of SVD(A)
+ * @param QL Left Householder accumulation (N×N)
+ * @param QR Right Householder accumulation (N×N)
+ * @param U Output: left singular vectors (N×N, first m×p used)
+ * @param Vt Output: right singular vectors transposed (N×N, first p×n used)
+ */
+template <uint8_t N>
+static void AssembleUAndVt(uint8_t m, uint8_t n, uint8_t p,
+                           bool transposeNeeded, const Matrix<N, N> &QL,
+                           const Matrix<N, N> &QR, Matrix<N, N> &U,
+                           Matrix<N, N> &Vt);
+
+/**
+ * @brief Compute a Givens rotation that zeros out y.
+ *
+ * Computes c, s such that:
+ *   [c  s] [x] = [r]
+ *   [-s c] [y]   [0]
+ * where r = sqrt(x² + y²).
+ *
+ * @param x First element
+ * @param y Second element (to be zeroed)
+ * @param c Output: cosine of rotation angle
+ * @param s Output: sine of rotation angle
+ */
+static void ComputeGivens(float x, float y, float &c, float &s);
+
+/**
+ * @brief Apply a Givens rotation from the left to rows i and j.
+ *
+ * Applies [c s; -s c] to rows i, j of W (columns startCol..endCol).
+ *
+ * @tparam N Working buffer size
+ * @param W Input/output: matrix to transform
+ * @param i First row index
+ * @param j Second row index
+ * @param c Cosine of rotation angle
+ * @param s Sine of rotation angle
+ * @param startCol First column to transform
+ * @param endCol Last column to transform
+ */
+template <uint8_t N>
+static void ApplyGivensLeft(Matrix<N, N> &W, uint8_t i, uint8_t j, float c,
+                            float s, uint8_t startCol, uint8_t endCol);
+
+/**
+ * @brief Apply a Givens rotation from the right to columns i and j.
+ *
+ * Applies [c -s; s c]ᵀ to columns i, j of W (rows startRow..endRow).
+ *
+ * @tparam N Working buffer size
+ * @param W Input/output: matrix to transform
+ * @param i First column index
+ * @param j Second column index
+ * @param c Cosine of rotation angle
+ * @param s Sine of rotation angle
+ * @param startRow First row to transform
+ * @param endRow Last row to transform
+ */
+template <uint8_t N>
+static void ApplyGivensRight(Matrix<N, N> &W, uint8_t i, uint8_t j, float c,
+                             float s, uint8_t startRow, uint8_t endRow);
+} // namespace SVD
+
+#ifndef SVD_H_
+#include "SVD.cpp"
+#endif