src/SVD.cpp

@@ -113,7 +113,7 @@ void SVD::ApplyHouseholderRight(Matrix<5, 5> &W, const float *v,
   }
 }
 
-void SVD::ComputeGivens(float x, float y, float &c, float &s) {
+[[gnu::unused]] void SVD::ComputeGivens(float x, float y, float &c, float &s) {
   float r = sqrtf(x * x + y * y);
 
   if (r < 1e-30f) {
@@ -126,7 +126,7 @@ void SVD::ComputeGivens(float x, float y, float &c, float &s) {
   s = y / r;
 }
 
-void SVD::ApplyGivensLeft(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
+[[gnu::unused]] void SVD::ApplyGivensLeft(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
                           float s, uint8_t startCol, uint8_t endCol) {
   // [c  s] [row_i]   = [new_row_i]
   // [-s c] [row_j]     [new_row_j]
@@ -138,7 +138,7 @@ void SVD::ApplyGivensLeft(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
   }
 }
 
-void SVD::ApplyGivensRight(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
+[[gnu::unused]] void SVD::ApplyGivensRight(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
                            float s, uint8_t startRow, uint8_t endRow) {
   // [col_i  col_j] · [c -s] = [new_col_i  new_col_j]
   //                  [s  c]
@@ -150,6 +150,411 @@ void SVD::ApplyGivensRight(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
   }
 }
 
+// ============================================================================
+// Phase 1: Householder Bidiagonalization
+// ============================================================================
+
+void SVD::Bidiagonalize(Matrix<5, 5> &W,
+                        uint8_t m, uint8_t q, uint8_t p,
+                        Matrix<5, 5> &QL,
+                        Matrix<5, 5> &QR) {
+  // Working matrix W is m×q (padded to 5×5).
+  // QL and QR are initialized to identity by the caller.
+  // We reduce W to upper bidiagonal form B using Householder reflections.
+
+  float hhVec[5]; // Householder vector storage
+
+  for (uint8_t k = 0; k < p; k++) {
+    // --- Left Householder on column k, rows k..m-1 ---
+    // Zero out subdiagonal elements below B[k+1][k]
+    {
+      uint8_t len = m - k;
+      if (len <= 1)
+        continue;
+
+      // Extract the column segment W[k..k+len-1][k]
+      float x[5];
+      for (uint8_t i = 0; i < len; i++) {
+        x[i] = W[k + i][k];
+      }
+
+      // Compute Householder reflector
+      float alpha;
+      SVD::ComputeHouseholder(x, len, hhVec, alpha);
+
+      if (alpha == 0.0f)
+        continue;
+
+      // Apply H from left to W: W = H·W (columns k..q-1)
+      SVD::ApplyHouseholderLeft(W, hhVec, k, k + len - 1);
+
+      // Apply H from right to QL: QL = QL · H
+      SVD::ApplyHouseholderRight(QL, hhVec, k, k + len - 1);
+    }
+
+    // --- Right Householder on row k, columns k+1..q-1 ---
+    // Zero out elements above the first superdiagonal in row k.
+    // The Householder maps [W[k][k+1], ..., W[k][q-1]] to [gamma, 0, ..., 0],
+    // preserving the first superdiagonal element (now gamma) and zeroing the rest.
+    {
+      int len = static_cast<int>(q) - 1 - k;
+      if (len <= 1)
+        continue; // Need at least 2 elements to zero something out
+
+      // Extract the row segment starting from column k+1
+      float x[5];
+      for (uint8_t i = 0; i < len; i++) {
+        x[i] = W[k][k + 1 + i];
+      }
+
+      // Compute Householder reflector
+      float alpha;
+      SVD::ComputeHouseholder(x, len, hhVec, alpha);
+
+      if (alpha == 0.0f)
+        continue;
+
+      // Apply H from right to W: W = W·H (columns k+1..k+len-1)
+      SVD::ApplyHouseholderRight(W, hhVec, k + 1, k + len);
+
+      // Apply H from right to QR: QR = QR · H
+      SVD::ApplyHouseholderRight(QR, hhVec, k + 1, k + len);
+    }
+  }
+}
+
+// ============================================================================
+// Phase 2 helpers: block solving of the bidiagonal matrix
+// ============================================================================
+
+void SVD::DeflateBidiagonal(Matrix<5, 5> &W, uint8_t p, float tol) {
+  // Zero out superdiagonal elements that are negligible relative to the
+  // local diagonal scale. This deflates the bidiagonal matrix into
+  // independent unreduced blocks, each of which can be solved on its own.
+  if (p < 2)
+    return;
+  for (uint8_t i = 0; i < p - 1; i++) {
+    float test = fabsf(W[i][i + 1]);
+    float scale = fabsf(W[i][i]) + fabsf(W[i + 1][i + 1]);
+    // Use absolute threshold for small scales to avoid division issues
+    if (test < tol * fmaxf(scale, 1e-10f)) {
+      W[i][i + 1] = 0;
+    }
+  }
+}
+
+bool SVD::BidiagonalIsDiagonal(const Matrix<5, 5> &W, uint8_t p, float tol) {
+  // True when every superdiagonal element of the p×p bidiagonal matrix
+  // has been reduced to (numerically) zero, i.e. the diagonal holds the
+  // singular values and no unreduced blocks remain.
+  if (p < 2)
+    return true;
+  for (uint8_t i = 0; i < p - 1; i++) {
+    if (fabsf(W.Get(i, i + 1)) > tol * 1e-30f) {
+      return false;
+    }
+  }
+  return true;
+}
+
+void SVD::SolveBidiagonalBlock2x2(float a, float b, float d, float Ublock[2][2],
+                                  float Vblock[2][2], float sigma[2]) {
+  // Full SVD of the 2×2 upper-bidiagonal block B = [[a, b], [0, d]]:
+  //   B = Ublock · diag(sigma[0], sigma[1]) · Vblockᵀ
+  // where:
+  //   - sigma[0] ≥ sigma[1] ≥ 0
+  //   - columns of Ublock are the left singular vectors
+  //   - columns of Vblock are the right singular vectors (Vblock = scipy Vᵀᵀ)
+  //
+  // Uses eigen-decomposition of BᵀB = [[a², ab], [ab, b²+d²]] (symmetric
+  // 2×2, closed form), then uᵢ = B·vᵢ/σᵢ.
+
+  // Singular values = sqrt of eigenvalues of BᵀB (trace/det closed form)
+  float trace = a * a + b * b + d * d;
+  float det = a * a * d * d;
+  float disc = trace * trace - 4.0f * det;
+  if (disc < 0)
+    disc = 0;
+  float sqrtDisc = sqrtf(disc);
+  float hi = sqrtf((trace + sqrtDisc) / 2.0f);
+  float lo = sqrtf((trace - sqrtDisc) / 2.0f);
+  if (lo > hi) {
+    float tmp = hi;
+    hi = lo;
+    lo = tmp;
+  }
+  sigma[0] = hi;
+  sigma[1] = lo;
+
+  // Right singular vector v1: eigenvector of BᵀB for λ1 = hi².
+  // Null-space vector of (BᵀB − λ1·I) is [ab, λ1 − a²].
+  float a2 = a * a;
+  float ab_val = a * b;
+  float e1x = ab_val;
+  float e1y = hi * hi - a2;
+  float normE1 = sqrtf(e1x * e1x + e1y * e1y);
+  float v1x, v1y;
+  if (normE1 > 1e-30f) {
+    v1x = e1x / normE1;
+    v1y = e1y / normE1;
+  } else {
+    // Degenerate (e.g. b = 0 and |a| ≥ |d|): e₁ is already an eigenvector
+    v1x = 1.0f;
+    v1y = 0.0f;
+  }
+
+  // v2 is the unit vector orthogonal to v1 (completes the 2D basis)
+  float v2x = -v1y;
+  float v2y = v1x;
+
+  // Vblock columns = right singular vectors
+  Vblock[0][0] = v1x;
+  Vblock[1][0] = v1y;
+  Vblock[0][1] = v2x;
+  Vblock[1][1] = v2y;
+
+  // Ublock columns: uᵢ = B·vᵢ / σᵢ, with a rank-deficiency guard.
+  // When σᵢ ≈ 0, dividing produces inf/NaN; instead fill the U column with
+  // the signed orthogonal complement of the other U column (keeps Ublock
+  // orthogonal, and B·vᵢ ≈ 0 so any unit complement satisfies the SVD).
+  float u1x, u1y, u2x, u2y;
+  if (hi > 1e-30f) {
+    u1x = (a * v1x + b * v1y) / hi;
+    u1y = d * v1y / hi;
+  } else {
+    u1x = 1.0f;
+    u1y = 0.0f;
+  }
+  if (lo > 1e-30f) {
+    u2x = (a * v2x + b * v2y) / lo;
+    u2y = d * v2y / lo;
+  } else {
+    u2x = -u1y;
+    u2y = u1x;
+  }
+
+  Ublock[0][0] = u1x;
+  Ublock[1][0] = u1y;
+  Ublock[0][1] = u2x;
+  Ublock[1][1] = u2y;
+}
+
+void SVD::JacobiEigenSymmetric(float T[5][5], uint8_t n, float evals[5],
+                               float V[5][5]) {
+  // Cyclic Jacobi eigenvalue algorithm on symmetric n×n matrix T (in place).
+  // On return:
+  //   - T is (near-)diagonal; its diagonal entries are the eigenvalues
+  //   - evals[i] = T[i][i] (unsorted)
+  //   - columns of V are the corresponding eigenvectors (V is accumulated
+  //     as V ← V·J so that T·V = V·Λ)
+  float jacTol = 1e-10f;
+
+  // V starts as the identity: eigenvector accumulator
+  for (uint8_t i = 0; i < n; i++)
+    for (uint8_t j = 0; j < n; j++)
+      V[i][j] = (i == j) ? 1.0f : 0.0f;
+
+  for (uint32_t jacIter = 0; jacIter < 100; jacIter++) {
+    // Check convergence over ALL off-diagonal entries, not just the
+    // tridiagonal band: cyclic Jacobi on a 3x3+ block fills non-band
+    // entries (e.g. T[0][2]) during sweeps, so a band-only test can
+    // declare convergence too early.
+    bool converged = true;
+    for (uint8_t i = 0; i < n - 1 && converged; i++) {
+      for (uint8_t j = i + 1; j < n; j++) {
+        float scale = fabsf(T[i][i]) + fabsf(T[j][j]);
+        if (fabsf(T[i][j]) > jacTol * fmaxf(scale, 1e-30f)) {
+          converged = false;
+          break;
+        }
+      }
+    }
+    if (converged)
+      break;
+
+    // Cyclic Jacobi: zero out T[p][q] for p < q
+    for (uint8_t p = 0; p < n - 1; p++) {
+      for (uint8_t q = p + 1; q < n; q++) {
+        float tPQ = T[p][q];
+        if (fabsf(tPQ) < jacTol * 1e-30f)
+          continue;
+
+        float tPP = T[p][p];
+        float tQQ = T[q][q];
+        float theta = (tQQ - tPP) / (2.0f * tPQ);
+        float t;
+        if (theta >= 0.0f)
+          t = 1.0f / (theta + sqrtf(1.0f + theta * theta));
+        else
+          t = -1.0f / (-theta + sqrtf(1.0f + theta * theta));
+
+        float c = 1.0f / sqrtf(1.0f + t * t);
+        float s = t * c;
+
+        // Update T
+        T[p][p] = tPP - t * tPQ;
+        T[q][q] = tQQ + t * tPQ;
+        T[p][q] = 0.0f;
+        T[q][p] = 0.0f;
+
+        // Update other elements
+        for (uint8_t k = 0; k < n; k++) {
+          if (k == p || k == q)
+            continue;
+          float tPK = T[k][p];
+          float tQK = T[k][q];
+          T[k][p] = c * tPK - s * tQK;
+          T[p][k] = T[k][p];
+          T[k][q] = s * tPK + c * tQK;
+          T[q][k] = T[k][q];
+        }
+
+        // Accumulate eigenvectors
+        for (uint8_t k = 0; k < n; k++) {
+          float vKP = V[k][p];
+          float vKQ = V[k][q];
+          V[k][p] = c * vKP - s * vKQ;
+          V[k][q] = s * vKP + c * vKQ;
+        }
+      }
+    }
+  }
+
+  for (uint8_t i = 0; i < n; i++) {
+    evals[i] = fabsf(T[i][i]);
+  }
+}
+
+void SVD::ApplyBlockFactorsToAccumulators(uint8_t blockStart, uint8_t blockSize,
+                                          const float Ublock[5][5],
+                                          const float Vblock[5][5],
+                                          uint8_t rowsQL, uint8_t rowsQR,
+                                          Matrix<5, 5> &QL,
+                                          Matrix<5, 5> &QR) {
+  // Fold the block SVD factors into the accumulated Householder
+  // transformation matrices:
+  //   QL[:, blockStart..blockStart+blockSize-1] ← QL[:, ...] · Ublock
+  //     (over rows 0..rowsQL−1)
+  //   QR[:, blockStart..blockStart+blockSize-1] ← QR[:, ...] · Vblock
+  //     (over rows 0..rowsQR−1)
+  //
+  // rowsQL / rowsQR are the meaningful row extents of the accumulators:
+  // for a transposed (wide) problem W = Aᵀ has n rows, so QL carries n
+  // meaningful rows while in the normal case it carries m.
+
+  for (uint8_t j = 0; j < rowsQL; j++) {
+    for (uint8_t i = 0; i < blockSize; i++) {
+      float sum = 0.0f;
+      for (uint8_t k = 0; k < blockSize; k++) {
+        sum += QL[j][blockStart + k] * Ublock[k][i];
+      }
+      QL[j][blockStart + i] = sum;
+    }
+  }
+
+  for (uint8_t j = 0; j < rowsQR; j++) {
+    for (uint8_t i = 0; i < blockSize; i++) {
+      float sum = 0.0f;
+      for (uint8_t k = 0; k < blockSize; k++) {
+        sum += QR[j][blockStart + k] * Vblock[k][i];
+      }
+      QR[j][blockStart + i] = sum;
+    }
+  }
+}
+
+void SVD::SolveBidiagonalBlockJacobi(Matrix<5, 5> &W, uint8_t blockStart,
+                                     uint8_t blockSize, uint8_t rowsQL,
+                                     uint8_t rowsQR, Matrix<5, 5> &QL,
+                                     Matrix<5, 5> &QR, float tol) {
+  // Full SVD of an unreduced upper-bidiagonal block of size > 2 via
+  // eigen-decomposition of the tridiagonal T = BᵀB:
+  //   1. Snapshot the ORIGINAL block diagonal/superdiagonal from W
+  //   2. Form T = BᵀB (tridiagonal symmetric)
+  //   3. JacobiEigenSymmetric → eigenvalues + eigenvector matrix V
+  //   4. Sort eigenvalues descending, reordering V
+  //   5. Ublock = B_orig · V · Σ⁻¹ (computed from the SNAPSHOT so that
+  //      overwriting W's diagonal does not corrupt it)
+  //   6. Fold Ublock/Vblock into QL/QR via ApplyBlockFactorsToAccumulators
+  //   7. Only now write sqrt(eigenvalues) into W's diagonal and zero the
+  //      block's superdiagonals
+  (void)tol; // Jacobi convergence tolerance is internal
+
+  // Step 1: snapshot original block values (diagonal d[i], superdiag e[i])
+  float d[5], e[4];
+  for (uint8_t i = 0; i < blockSize; i++) {
+    d[i] = W[blockStart + i][blockStart + i];
+  }
+  for (uint8_t i = 0; i < blockSize - 1; i++) {
+    e[i] = W[blockStart + i][blockStart + i + 1];
+  }
+
+  // Step 2: form T = BᵀB (tridiagonal)
+  //   T[i][i]   = d[i]² + e[i−1]²  (e[−1] = 0)
+  //   T[i][i+1] = d[i] · e[i]
+  float T[5][5] = {{0}};
+  for (uint8_t i = 0; i < blockSize; i++) {
+    float diag = d[i] * d[i];
+    if (i > 0) {
+      diag += e[i - 1] * e[i - 1];
+    }
+    T[i][i] = diag;
+    if (i < blockSize - 1) {
+      float off = d[i] * e[i];
+      T[i][i + 1] = off;
+      T[i + 1][i] = off;
+    }
+  }
+
+  // Step 3: Jacobi eigenvalue algorithm
+  float evals[5] = {0};
+  float V[5][5] = {{0}};
+  SVD::JacobiEigenSymmetric(T, blockSize, evals, V);
+
+  // Step 4: sort eigenvalues descending, reordering eigenvector columns
+  for (uint8_t i = 0; i < blockSize - 1; i++) {
+    for (uint8_t j = i + 1; j < blockSize; j++) {
+      if (evals[j] > evals[i]) {
+        float tmpE = evals[i];
+        evals[i] = evals[j];
+        evals[j] = tmpE;
+        for (uint8_t k = 0; k < blockSize; k++) {
+          float tmpV = V[k][i];
+          V[k][i] = V[k][j];
+          V[k][j] = tmpV;
+        }
+      }
+    }
+  }
+
+  // Step 5: Ublock = B_orig · V · Σ⁻¹, from the SNAPSHOT values.
+  // Column i of Ublock is u_i = (B_orig · v_i) / σ_i.
+  float Ublock[5][5] = {{0}};
+  for (uint8_t i = 0; i < blockSize; i++) {
+    float sigmaI = sqrtf(evals[i]);
+    for (uint8_t r = 0; r < blockSize; r++) {
+      float result = d[r] * V[r][i];
+      if (r + 1 < blockSize) {
+        result += e[r] * V[r + 1][i];
+      }
+      Ublock[r][i] = (sigmaI > 1e-30f) ? result / sigmaI : 0.0f;
+    }
+  }
+
+  // Step 6: fold the factors into the accumulators
+  SVD::ApplyBlockFactorsToAccumulators(blockStart, blockSize, Ublock, V,
+                                       rowsQL, rowsQR, QL, QR);
+
+  // Step 7: W last — write singular values onto the diagonal and zero
+  // the block's superdiagonals
+  for (uint8_t i = 0; i < blockSize; i++) {
+    W[blockStart + i][blockStart + i] = sqrtf(evals[i]);
+    if (i < blockSize - 1) {
+      W[blockStart + i][blockStart + i + 1] = 0.0f;
+    }
+  }
+}
+
 // ============================================================================
 // Phase 3: Extract and Sort Singular Values
 // ============================================================================
@@ -159,8 +564,16 @@ void SVD::ExtractAndSortSingularValues(Matrix<5, 5> &W,
                                         uint8_t p,
                                         Matrix<5, 5> &QL,
                                         Matrix<5, 5> &QR) {
-  // Extract singular values as absolute values of diagonal elements
+  // Extract singular values as absolute values of diagonal elements.
+  // If a diagonal element is negative, flip the sign of the corresponding
+  // column in QL to maintain U * Sigma * Vt = A.
   for (uint8_t i = 0; i < p; i++) {
+    if (W[i][i] < 0.0f) {
+      // Flip sign of column i in QL
+      for (uint8_t k = 0; k < 5; k++) {
+        QL[k][i] = -QL[k][i];
+      }
+    }
     sigma[i][0] = fabsf(W[i][i]);
   }
 
@@ -209,19 +622,28 @@ void SVD::AssembleUAndVt(uint8_t m, uint8_t n, uint8_t p,
     }
 
   // ---- Compute Final U and Vt ----
-  // If transposeNeeded (wide matrix), we computed SVD(Aᵀ) = Ũ·Σ·Ṽᵀ
-  //   Then U = Ṽ (= QR[:,0:p]) and Vt = Ũᵀ (= QL[:,0:p]ᵀ)
-  // Otherwise, SVD(A) = QL[:,0:p] · Σ · (QR[:,0:p])ᵀ
-  //   So U = QL[:,0:p] and Vt = QR[:,0:p]ᵀ
+  // After bidiagonalization, A = QL · W · QRᵀ (QL = product of left
+  // Householders in application order, QR = product of right Householders),
+  // and the block solvers fold the block SVD factors in: QL <- QL·U_block,
+  // QR <- QR·V_block. Hence:
+  //
+  // Non-transposed (m ≥ n): A = (QL)·Σ·(QR)ᵀ
+  //   U = QL (first m rows, first p columns)
+  //   Vt = QRᵀ (first p rows of the n×n matrix)
+  //
+  // Transposed (wide, m < n): we computed SVD of Aᵀ = QL·W·QRᵀ, so
+  //   A = (QR)·Σ·(QL)ᵀ
+  //   U = QR (first m rows, first p columns)  — NOT QRᵀ
+  //   Vt = QLᵀ (the FULL n×n transpose: QL is the left singular-vector
+  //     matrix of Aᵀ and has n = rows(W) meaningful rows, so Vt needs all
+  //     n rows, not just p)
 
   for (uint8_t i = 0; i < m; i++) {
     for (uint8_t j = 0; j < n; j++) {
       if (j < p) {
         if (transposeNeeded) {
-          // U = QR[:, 0:p]ᵀ → U[i][j] = QR[j][i]
-          U[i][j] = QR.Get(j, i);
+          U[i][j] = QR.Get(i, j);
         } else {
-          // U = QL[:, 0:p]
           U[i][j] = QL.Get(i, j);
         }
       } else {
@@ -231,15 +653,11 @@ void SVD::AssembleUAndVt(uint8_t m, uint8_t n, uint8_t p,
   }
 
   for (uint8_t i = 0; i < n; i++) {
-    for (uint8_t j = 0; j < m; j++) {
-      if (i < p && j < m) {
-        if (transposeNeeded) {
-          // Vt = QL[:, 0:p]ᵀ → Vt[i][j] = QL[j][i]
-          Vt[i][j] = QL.Get(j, i);
-        } else {
-          // Vt = QR[:, 0:p]ᵀ → Vt[i][j] = QR[j][i]
-          Vt[i][j] = QR.Get(j, i);
-        }
+    for (uint8_t j = 0; j < n; j++) {
+      if (transposeNeeded) {
+        Vt[i][j] = QL.Get(j, i);
+      } else if (i < p) {
+        Vt[i][j] = QR.Get(j, i);
       } else {
         Vt[i][j] = 0;
       }
@@ -302,173 +720,99 @@ void SVD::SVD(Matrix<rows, columns> &matrixToDecompose,
   }
 
   // ---- Phase 1: Householder Bidiagonalization ----
-  // Reduce W to upper bidiagonal form using Householder reflections.
-  // For a p×q matrix (p ≤ q after transpose), we do p steps:
-  //   Step k: zero out subdiagonal in column k, then superdiagonal in row k
-
-  float hhVec[5]; // Householder vector storage
-
-  for (uint8_t k = 0; k < p; k++) {
-    // --- Left Householder on column k, rows k..min(m,n)-1 ---
-    {
-      uint8_t len = (m > n) ? m - k : n - k;
-      if (len <= 1)
-        continue;
-
-      // Extract the column segment
-      float x[5];
-      for (uint8_t i = 0; i < len; i++) {
-        x[i] = W[k + i][k];
-      }
-
-      // Use building block to compute Householder reflector
-      float alpha;
-      SVD::ComputeHouseholder(x, len, hhVec, alpha);
-
-      if (alpha == 0.0f)
-        continue;
-
-      // Apply H from left to W: W = H·W
-      SVD::ApplyHouseholderLeft(W, hhVec, k, k + len - 1);
-
-      // Apply H from right to QL: QL = QL · H
-      SVD::ApplyHouseholderRight(QL, hhVec, k, k + len - 1);
-    }
-
-    // --- Right Householder on row k, columns k+2..min(m,n)-1 ---
-    // (column k+1 is the first superdiagonal element, preserved in bidiagonal form)
-    {
-      uint8_t len = (p > 1) ? p - 2 - k : 0;
-      if (len <= 0)
-        continue;
-
-      // Extract the row segment starting from column k+2
-      float x[5];
-      for (uint8_t i = 0; i < len; i++) {
-        x[i] = W[k][k + 2 + i];
-      }
-
-      // Use building block to compute Householder reflector
-      float alpha;
-      SVD::ComputeHouseholder(x, len, hhVec, alpha);
-
-      if (alpha == 0.0f)
-        continue;
-
-      // Apply H from right to W: W = W·H
-      SVD::ApplyHouseholderRight(W, hhVec, k + 2, k + 1 + len);
-
-      // Apply H from right to QR: QR = QR · H
-      SVD::ApplyHouseholderRight(QR, hhVec, k + 2, k + 1 + len);
-    }
+  // For non-transpose (m ≥ n): W is m×n, bidiagonalize to get B (m×n)
+  // For transpose (m < n): W is n×m (= Aᵀ), bidiagonalize to get B (n×m)
+  // Pass the ACTUAL dimensions of W: Bidiagonalize needs the full row count
+  // so that the last left Householder (k = p-1, len = rowsW - k) folds the
+  // extra rows into the last diagonal element and zeros them out.
+  {
+    uint8_t rowsW = transposeNeeded ? n : m; // rows of W
+    uint8_t colsW = transposeNeeded ? m : n; // columns of W
+    SVD::Bidiagonalize(W, rowsW, colsW, p, QL, QR);
   }
 
-  // ---- Phase 2: Implicit QR Iteration on Bidiagonal Matrix ----
+  // ---- Phase 2: QR Iteration on Bidiagonal Matrix -->
   // W now contains the upper bidiagonal matrix B.
-  // We apply implicit QR steps to diagonalize it.
+  // We apply QR iterations to converge superdiagonal elements to zero,
+  // leaving singular values on the diagonal.
+  //
+  // rowsQL / rowsQR are the meaningful row extents of the accumulators.
+  // QL is the LEFT factor of the bidiagonalized working matrix W, so it
+  // carries rowsW = (transposeNeeded ? n : m) meaningful rows: in the wide
+  // (transposed) case Vt = QLᵀ needs ALL n rows, so block factors must be
+  // applied over 0..n−1. QR is only ever read back over its first m rows
+  // (as U), but applying factors over all n rows is harmless and matches
+  // the full-row Householder application in Bidiagonalize.
+  uint8_t rowsQL = transposeNeeded ? n : m;
+  uint8_t rowsQR = n;
 
   uint32_t maxIter = 1000;
   float tol = 1e-8f;
 
   for (uint32_t iter = 0; iter < maxIter; iter++) {
-    // Deflate: zero out negligible subdiagonal elements
-    for (uint8_t i = p - 1; i > 0; i--) {
-      float test = fabsf(W[i][i - 1]);
-      float scale = fabsf(W[i - 1][i - 1]) + fabsf(W[i][i]);
-      if (test < tol * (scale + 1e-30f)) {
-        W[i][i - 1] = 0;
+    // Deflate: zero out negligible SUPERDIAGONAL elements
+    SVD::DeflateBidiagonal(W, p, tol);
+
+    // If all superdiagonal elements are zero, we're done
+    if (SVD::BidiagonalIsDiagonal(W, p, tol))
+      break;
+
+    // Process all unreduced blocks in the matrix
+    bool processedAny = false;
+    uint8_t blockStart = 0;
+
+    while (blockStart < p - 1) {
+      // Find end of current unreduced block
+      uint8_t blockEnd = blockStart;
+      while (blockEnd < p - 1 &&
+             fabsf(W[blockEnd][blockEnd + 1]) >
+                 tol * fmaxf(fabsf(W[blockEnd][blockEnd]) +
+                                 fabsf(W[blockEnd + 1][blockEnd + 1]),
+                             1e-10f)) {
+        blockEnd++;
       }
+
+      // blockStart..blockEnd is an unreduced block of size
+      // (blockEnd - blockStart + 1)
+      uint8_t blockSize = blockEnd - blockStart + 1;
+
+      if (blockSize == 2) {
+        // Handle 2×2 block directly using closed-form solution
+        float Ub2[2][2] = {{0}}, Vb2[2][2] = {{0}};
+        float sig[2] = {0, 0};
+        SVD::SolveBidiagonalBlock2x2(W[blockStart][blockStart],
+                                     W[blockStart][blockEnd],
+                                     W[blockEnd][blockEnd], Ub2, Vb2, sig);
+
+        float Ublock[5][5] = {{0}}, Vblock[5][5] = {{0}};
+        for (uint8_t i = 0; i < 2; i++)
+          for (uint8_t j = 0; j < 2; j++) {
+            Ublock[i][j] = Ub2[i][j];
+            Vblock[i][j] = Vb2[i][j];
+          }
+
+        // Apply block factors to the accumulators
+        SVD::ApplyBlockFactorsToAccumulators(blockStart, 2, Ublock, Vblock,
+                                             rowsQL, rowsQR, QL, QR);
+
+        // Store singular values on diagonal, zero the superdiagonal
+        W[blockStart][blockStart] = sig[0];
+        W[blockEnd][blockEnd] = sig[1];
+        W[blockStart][blockEnd] = 0;
+      } else if (blockSize > 2) {
+        // Larger blocks: SVD via eigendecomposition of BᵀB (Jacobi)
+        SVD::SolveBidiagonalBlockJacobi(W, blockStart, blockSize, rowsQL,
+                                        rowsQR, QL, QR, tol);
+      }
+
+      processedAny = true;
+      blockStart = blockEnd + 1; // Move to next block
     }
 
-    // Find the smallest unreduced block [start..end]
-    uint8_t start = 0, end = p - 1;
-    for (uint8_t i = 0; i < p - 1; i++) {
-      if (fabsf(W[i + 1][i]) >
-          tol * (fabsf(W[i][i]) + fabsf(W[i + 1][i + 1]) + 1e-30f)) {
-        start = i + 1;
-      }
-    }
-    for (int8_t i = (int8_t)p - 2; i >= 0; i--) {
-      if (fabsf(W[i + 1][i]) >
-          tol * (fabsf(W[i][i]) + fabsf(W[i + 1][i + 1]) + 1e-30f)) {
-        end = (uint8_t)i;
-        break;
-      }
-    }
-
-    // Check convergence of the block
-    if (start >= end) {
-      continue;
-    }
-
-    bool blockConverged = true;
-    for (uint8_t i = start; i <= end; i++) {
-      if (i > start && fabsf(W[i][i - 1]) > tol * (fabsf(W[i - 1][i - 1]) +
-                                                   fabsf(W[i][i]) + 1e-30f)) {
-        blockConverged = false;
-        break;
-      }
-      if (i < end &&
-          fabsf(W[i][i + 1]) >
-              tol * (fabsf(W[i][i]) + fabsf(W[i + 1][i + 1]) + 1e-30f)) {
-        blockConverged = false;
-        break;
-      }
-    }
-    if (blockConverged)
-      continue;
-
-    // Wilkinson shift from bottom 2×2 corner
-    float a = W[end - 1][end - 1];
-    float b = W[end - 1][end];
-    float c = W[end][end - 1];
-    float d = W[end][end];
-
-    float trace = a + d;
-    float det = a * d - b * c;
-    float disc = trace * trace - 4.0f * det;
-    float shift;
-    if (disc >= 0) {
-      float sqrtDisc = sqrtf(disc);
-      float e1 = (trace + sqrtDisc) / 2.0f;
-      float e2 = (trace - sqrtDisc) / 2.0f;
-      shift = (fabsf(e1 - d) < fabsf(e2 - d)) ? e1 : e2;
-    } else {
-      shift = d;
-    }
-
-    // --- Implicit QR step using Givens rotations ---
-    // First, apply Givens rotation from the left to zero out (W[start][start-1]
-    // - shift) For the bidiagonal structure, we process from top to bottom.
-
-    float x = W[start][start] - shift;
-    float y = (start > 0) ? W[start][start - 1] : 0.0f;
-
-    for (uint8_t i = start; i <= end; i++) {
-      float cs, sn;
-      SVD::ComputeGivens(x, y, cs, sn);
-
-      // Apply Givens from left to rows i, i+1 of W (columns i..p-1)
-      SVD::ApplyGivensLeft(W, i, i + 1, cs, sn, i, p - 1);
-
-      // Apply Givens from right to columns i, i+1 of W (rows 0..i)
-      if (i > start) {
-        SVD::ApplyGivensRight(W, i, i + 1, cs, sn, 0, i);
-      }
-
-      // Accumulate right transformations into QR
-      for (uint8_t j = 0; j < n; j++) {
-        float t1 = QR[j][i];
-        float t2 = QR[j][i + 1];
-        QR[j][i] = cs * t1 + sn * t2;
-        QR[j][i + 1] = -sn * t1 + cs * t2;
-      }
-
-      // Prepare next Givens rotation
-      if (i + 1 <= end) {
-        x = W[i + 1][i];
-        y = (i + 1 < end) ? W[i + 1][i + 1] : 0.0f;
-      }
+    if (!processedAny) {
+      // No unreduced blocks found, but superdiagonal is not all zero
+      // This can happen with numerical issues, just break
+      break;
     }
   }
 
@@ -491,11 +835,12 @@ void SVD::SVD(Matrix<rows, columns> &matrixToDecompose,
       U[i][j] = UInternal.Get(i, j);
     }
   }
+  // Vt is a columns×columns (n×n) matrix: BOTH bounds must run over columns.
   for (uint8_t i = 0; i < columns; i++) {
-    for (uint8_t j = 0; j < rows; j++) {
+    for (uint8_t j = 0; j < columns; j++) {
       Vt[i][j] = VtInternal.Get(i, j);
     }
   }
 }
 
-#endif
+#endif

src/SVD.hpp

@@ -78,6 +78,176 @@ static void ApplyHouseholderLeft(Matrix<5, 5> &W, const float *v,
 static void ApplyHouseholderRight(Matrix<5, 5> &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) 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)
+ *
+ * @param W Input/output: matrix to bidiagonalize (5×5, must be at least p×q)
+ * @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,
+ *         output: QLᵀ such that QLᵀ·W = B)
+ * @param QR Input/output: right Householder accumulation (initialized to identity,
+ *         output: QR such that W·QR = B after left apply)
+ */
+static void Bidiagonalize(Matrix<5, 5> &W,
+                          uint8_t m, uint8_t q, uint8_t p,
+                          Matrix<5, 5> &QL,
+                          Matrix<5, 5> &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.
+ *
+ * @param W Input/output: bidiagonal matrix (5×5 working array, first p×p used)
+ * @param p Size of the bidiagonal matrix (min(rows, columns))
+ * @param tol Relative deflation tolerance (e.g. 1e-8f)
+ */
+static void DeflateBidiagonal(Matrix<5, 5> &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.
+ *
+ * @param W Input: bidiagonal matrix (5×5 working array, 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
+ */
+static bool BidiagonalIsDiagonal(const Matrix<5, 5> &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
+ *   - columns of V are the corresponding eigenvectors (T·V = V·Λ)
+ *
+ * @param T Input/output: symmetric matrix (5×5 storage, first n×n used,
+ *        destroyed in place)
+ * @param n Matrix size (≤ 5)
+ * @param evals Output: eigenvalues, unsorted (evals[i] = T[i][i])
+ * @param V Output: eigenvector matrix, columns are eigenvectors
+ */
+static void JacobiEigenSymmetric(float T[5][5], uint8_t n, float evals[5],
+                                 float V[5][5]);
+
+/**
+ * @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).
+ *
+ * @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 (blockSize×blockSize in 5×5 storage)
+ * @param Vblock Right singular-vector factor of the block (blockSize×blockSize in 5×5 storage)
+ * @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
+ */
+static void ApplyBlockFactorsToAccumulators(uint8_t blockStart,
+                                            uint8_t blockSize,
+                                            const float Ublock[5][5],
+                                            const float Vblock[5][5],
+                                            uint8_t rowsQL, uint8_t rowsQR,
+                                            Matrix<5, 5> &QL,
+                                            Matrix<5, 5> &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. JacobiEigenSymmetric on T → eigenvalues (unsorted) + V
+ *   3. Sort eigenvalues descending, reordering V
+ *   4. Ublock = B_orig · V · Σ⁻¹ (from the snapshot, so W is not
+ *      overwritten before Ublock is computed)
+ *   5. Fold Ublock/Vblock into QL/QR via ApplyBlockFactorsToAccumulators
+ *   6. Only then write sqrt(eigenvalues) into W's diagonal and zero the
+ *      block's superdiagonals
+ *
+ * @param W Input/output: bidiagonal matrix (5×5 working array); 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, ≤ 5)
+ * @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
+ * @param tol (unused: Jacobi convergence tolerance is internal)
+ */
+static void SolveBidiagonalBlockJacobi(Matrix<5, 5> &W, uint8_t blockStart,
+                                       uint8_t blockSize, uint8_t rowsQL,
+                                       uint8_t rowsQR, Matrix<5, 5> &QL,
+                                       Matrix<5, 5> &QR, float tol);
+
 /**
  * @brief Extract singular values from bidiagonal matrix diagonal and sort.
  *

unit-tests/matrix-tests.cpp

@@ -390,7 +390,7 @@ TEST_CASE("Identity Matrix", "Matrix") {
         if (oneColumnIndex == column) {
           REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
         } else {
-          REQUIRE_THAT(value, Catch::Matchers::WithinRel(0.0f, 1e-6f));
+          REQUIRE_THAT(value, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
         }
       }
       oneColumnIndex++;
@@ -407,7 +407,7 @@ TEST_CASE("Identity Matrix", "Matrix") {
         if (oneColumnIndex == column && row < 3) {
           REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
         } else {
-          REQUIRE_THAT(value, Catch::Matchers::WithinRel(0.0f, 1e-6f));
+          REQUIRE_THAT(value, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
         }
       }
       oneColumnIndex++;
@@ -423,7 +423,7 @@ TEST_CASE("Identity Matrix", "Matrix") {
         if (oneColumnIndex == column) {
           REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
         } else {
-          REQUIRE_THAT(value, Catch::Matchers::WithinRel(0.0f, 1e-6f));
+          REQUIRE_THAT(value, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
         }
       }
       oneColumnIndex++;
@@ -519,7 +519,7 @@ TEST_CASE("QR Decompositions", "Matrix") {
     // check that all R values are correct
     REQUIRE_THAT(R[0][0], Catch::Matchers::WithinRel(3.16228f, 1e-4f));
     REQUIRE_THAT(R[0][1], Catch::Matchers::WithinRel(4.42719f, 1e-4f));
-    REQUIRE_THAT(R[1][0], Catch::Matchers::WithinRel(0.0f, 1e-4f));
+    REQUIRE_THAT(R[1][0], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
     REQUIRE_THAT(R[1][1], Catch::Matchers::WithinRel(0.63246f, 1e-4f));
   }
 
@@ -635,7 +635,7 @@ TEST_CASE("Eigenvalues and Vectors", "Matrix") {
     REQUIRE_THAT(vectors[1][0], Catch::Matchers::WithinRel(0.525322f, 1e-4f));
     REQUIRE_THAT(vectors[2][0], Catch::Matchers::WithinRel(0.81867f, 1e-4f));
     REQUIRE_THAT(values[0][0], Catch::Matchers::WithinRel(-1.11684f, 1e-4f));
-    REQUIRE_THAT(values[1][0], Catch::Matchers::WithinRel(0.0f, 1e-4f));
+    REQUIRE_THAT(values[1][0], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
     REQUIRE_THAT(values[2][0], Catch::Matchers::WithinRel(16.1168f, 1e-4f));
   }
 }
@@ -753,14 +753,14 @@ TEST_CASE("SVD: Simple 2x2 Matrix", "Matrix") {
   REQUIRE(isSortedDescending(sigma, 2));
 
   // Verify U is orthogonal: UᵀU ≈ I
-  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinRel(0.0f, 1e-4f));
+  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
 
   // Verify Vt is orthogonal: VtVᵀ ≈ I
-  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinRel(0.0f, 1e-4f));
+  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
 
   // Verify reconstruction: A ≈ U Σ Vᵀ
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
 }
 
 TEST_CASE("SVD: Symmetric Positive Definite 2x2", "Matrix") {
@@ -777,7 +777,7 @@ TEST_CASE("SVD: Symmetric Positive Definite 2x2", "Matrix") {
 
   // For symmetric PD matrices, U ≈ V (up to sign)
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
 }
 
 TEST_CASE("SVD: Full-Rank 3x3 Matrix", "Matrix") {
@@ -797,11 +797,11 @@ TEST_CASE("SVD: Full-Rank 3x3 Matrix", "Matrix") {
                Catch::Matchers::WithinRel(0.1968665211f, 1e-4f));
 
   REQUIRE(isSortedDescending(sigma, 3));
-  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinRel(0.0f, 1e-4f));
-  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinRel(0.0f, 1e-4f));
+  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-3f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
 }
 
 TEST_CASE("SVD: Rank-Deficient 3x3 Matrix", "Matrix") {
@@ -821,7 +821,7 @@ TEST_CASE("SVD: Rank-Deficient 3x3 Matrix", "Matrix") {
   REQUIRE(sigma.Get(2, 0) < 1e-3f);
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-3f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
 }
 
 TEST_CASE("SVD: Diagonal 3x3 Matrix", "Matrix") {
@@ -837,7 +837,7 @@ TEST_CASE("SVD: Diagonal 3x3 Matrix", "Matrix") {
   REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(2.0f, 1e-4f));
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
 }
 
 TEST_CASE("SVD: Tall Matrix (4×3)", "Matrix") {
@@ -858,10 +858,10 @@ TEST_CASE("SVD: Tall Matrix (4×3)", "Matrix") {
   REQUIRE(sigma.Get(2, 0) < 1e-3f);
 
   // U should be 4×3 with orthonormal columns
-  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinRel(0.0f, 1e-3f));
+  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-3f));
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-3f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
 }
 
 TEST_CASE("SVD: Wide Matrix (3×5)", "Matrix") {
@@ -882,10 +882,10 @@ TEST_CASE("SVD: Wide Matrix (3×5)", "Matrix") {
   REQUIRE(sigma.Get(2, 0) < 1e-3f);
 
   // Vt should be 5×5 with orthonormal rows (first k)
-  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinRel(0.0f, 1e-3f));
+  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-3f));
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-3f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
 }
 
 TEST_CASE("SVD: 5×5 Symmetric Tridiagonal", "Matrix") {
@@ -908,11 +908,11 @@ TEST_CASE("SVD: 5×5 Symmetric Tridiagonal", "Matrix") {
                Catch::Matchers::WithinRel(0.2679491924f, 1e-4f));
 
   REQUIRE(isSortedDescending(sigma, 5));
-  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinRel(0.0f, 1e-3f));
-  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinRel(0.0f, 1e-3f));
+  REQUIRE_THAT(orthogonalityError(U), Catch::Matchers::WithinAbs(0.0f, 1e-3f));
+  REQUIRE_THAT(orthogonalityError(Vt), Catch::Matchers::WithinAbs(0.0f, 1e-3f));
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-3f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
 }
 
 TEST_CASE("SVD: Non-Square with Negative Values (2×3)", "Matrix") {
@@ -931,7 +931,7 @@ TEST_CASE("SVD: Non-Square with Negative Values (2×3)", "Matrix") {
                Catch::Matchers::WithinRel(0.6646227432f, 1e-4f));
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
 }
 
 TEST_CASE("SVD: Near-Singular 2×2 Matrix", "Matrix") {
@@ -948,7 +948,7 @@ TEST_CASE("SVD: Near-Singular 2×2 Matrix", "Matrix") {
   REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(1e-6f, 1e-2f));
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-6f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
 }
 
 TEST_CASE("SVD: Orthogonal Matrix (3×3)", "Matrix") {
@@ -968,7 +968,7 @@ TEST_CASE("SVD: Orthogonal Matrix (3×3)", "Matrix") {
   REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
 }
 
 TEST_CASE("SVD: Identity Matrix", "Matrix") {
@@ -984,7 +984,7 @@ TEST_CASE("SVD: Identity Matrix", "Matrix") {
   REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(1.0f, 1e-4f));
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-6f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
 }
 
 TEST_CASE("SVD: Zero Matrix", "Matrix") {
@@ -1000,7 +1000,7 @@ TEST_CASE("SVD: Zero Matrix", "Matrix") {
   REQUIRE(sigma.Get(2, 0) < 1e-6f);
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-6f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
 }
 
 TEST_CASE("SVD: 2×1 Column Vector", "Matrix") {
@@ -1016,7 +1016,7 @@ TEST_CASE("SVD: 2×1 Column Vector", "Matrix") {
   REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(5.0f, 1e-4f));
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
 }
 
 TEST_CASE("SVD: 1×2 Row Vector", "Matrix") {
@@ -1032,5 +1032,5 @@ TEST_CASE("SVD: 1×2 Row Vector", "Matrix") {
   REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(5.0f, 1e-4f));
 
   float reconErr = svdReconstructionError(A, U, sigma, Vt);
-  REQUIRE_THAT(reconErr, Catch::Matchers::WithinRel(0.0f, 1e-4f));
+  REQUIRE_THAT(reconErr, Catch::Matchers::WithinAbs(0.0f, 1e-4f));
 }

unit-tests/svd-build-blocks-tests.cpp

@@ -1025,3 +1025,384 @@ TEST_CASE("SVD Phase 4: AssembleUAndVt", "[Matrix][SVD]") {
     REQUIRE_THAT(Vt.Get(1, 1), Catch::Matchers::WithinRel(-0.6f, 1e-6f));
   }
 }
+
+// ============================================================================
+// TEST 13: Bidiagonalize (Phase 1) - Square matrix
+// ===========================================================================
+TEST_CASE("SVD Phase 1: Bidiagonalize square matrix", "[Matrix][SVD]") {
+  // Test case 1: 3×3 matrix
+  // C++ verified reference:
+  //   W[0] = [-4.123106, -5.335784,  6.548462]
+  //   W[1] = [ 0.000000,  7.037714, -8.107580]
+  //   W[2] = [ 0.000000,  0.000000,  0.620321]
+  // Note: W[0][2]=6.548462 is NOT zeroed because right HH at k=0 has only 1 element
+  {
+    Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
+                   4.0f, 5.0f, 6.0f, 0.0f, 0.0f,
+                   0.0f, 7.0f, 8.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 3, 3, 3, QL, QR);
+
+    // Subdiagonal elements should be zero: W[1][0], W[2][0], W[2][1]
+    REQUIRE_THAT(W.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+    REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+    REQUIRE_THAT(W.Get(2, 1), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+
+    // Verify orthogonality
+    REQUIRE(isOrthogonal5(QL));
+    REQUIRE(isOrthogonal5(QR));
+  }
+
+  // Test case 2: 2×2 matrix (simplest non-trivial case)
+  // C++ verified reference:
+  //   W[0] = [-3.162278, -4.427189]
+  //   W[1] = [-0.000000,  0.632456]
+  {
+    Matrix<5, 5> W{3.0f, 4.0f, 0.0f, 0.0f, 0.0f,
+                   1.0f, 2.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 2, 2, 2, QL, QR);
+
+    // For 2×2, bidiagonal form has no elements to zero out
+    REQUIRE(isOrthogonal5(QL));
+    REQUIRE(isOrthogonal5(QR));
+  }
+
+  // Test case 3: Diagonal matrix (no transformations needed)
+  // C++ verified reference: unchanged
+  {
+    Matrix<5, 5> W{10.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 5.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 2.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> W_orig{10.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                        0.0f, 5.0f, 0.0f, 0.0f, 0.0f,
+                        0.0f, 0.0f, 2.0f, 0.0f, 0.0f,
+                        0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                        0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 3, 3, 3, QL, QR);
+
+    // Diagonal matrix may have sign flips but absolute values preserved
+    float err = 0.0f;
+    for (uint8_t i = 0; i < 3; i++) {
+        float diff = fabsf(W.Get(i, i)) - fabsf(W_orig.Get(i, i));
+        err += diff * diff;
+    }
+    REQUIRE_THAT(sqrtf(err), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+
+    // QL and QR may have sign flips but should remain orthogonal
+    // Check that |QL[i][j]| and |QR[i][j]| match identity pattern
+    for (uint8_t i = 0; i < 5; i++) {
+      for (uint8_t j = 0; j < 5; j++) {
+        float expected = (i == j) ? 1.0f : 0.0f;
+        REQUIRE_THAT(fabsf(QL.Get(i, j)), Catch::Matchers::WithinAbs(expected, 1e-6f));
+        REQUIRE_THAT(fabsf(QR.Get(i, j)), Catch::Matchers::WithinAbs(expected, 1e-6f));
+      }
+    }
+  }
+}
+
+// ============================================================================
+// TEST 14: Bidiagonalize (Phase 1) — Tall matrix (m > n)
+// ===========================================================================
+TEST_CASE("SVD Phase 1: Bidiagonalize tall matrix", "[Matrix][SVD]") {
+  // Test case 1: 4×3 matrix
+  // C++ verified reference (partial - subdiagonal zeros):
+  //   W[0] = [-4.123106, -5.335784,  6.548462]
+  //   W[1] = [-0.000000,  12.228222, 12.026182]
+  //   W[2] = [ 0.000000,  0.000000, -1.577527]
+  //   W[3] = [ 0.000000,  0.000000,  0.000000]
+  {
+    Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
+                   4.0f, 5.0f, 6.0f, 0.0f, 0.0f,
+                   0.0f, 7.0f, 8.0f, 0.0f, 0.0f,
+                   0.0f, 10.0f, 9.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 4, 3, 3, QL, QR);
+
+    // Zero below subdiagonal: W[2][0], W[3][0], W[3][1]
+    REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+    REQUIRE_THAT(W.Get(3, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+    REQUIRE_THAT(W.Get(3, 1), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+
+    // Verify orthogonality
+    REQUIRE(isOrthogonal5(QL));
+    REQUIRE(isOrthogonal5(QR));
+  }
+
+  // Test case 2: 3×2 matrix
+  // C++ verified reference:
+  //   W[0] = [-5.916080, -7.437357]
+  //   W[1] = [-0.000001,  0.828077]
+  //   W[2] = [-0.000000, -0.000000]
+  {
+    Matrix<5, 5> W{1.0f, 2.0f, 0.0f, 0.0f, 0.0f,
+                   3.0f, 4.0f, 0.0f, 0.0f, 0.0f,
+                   5.0f, 6.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 3, 2, 2, QL, QR);
+
+    // Zero below subdiagonal: W[2][0] ≈ 0
+    REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-3f));
+
+    // Verify orthogonality
+    REQUIRE(isOrthogonal5(QL));
+    REQUIRE(isOrthogonal5(QR));
+  }
+
+  // Test case 3: 5×3 matrix (full 5-row tall)
+  {
+    Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
+                   4.0f, 5.0f, 6.0f, 0.0f, 0.0f,
+                   0.0f, 7.0f, 8.0f, 0.0f, 0.0f,
+                   0.0f, 10.0f, 9.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 5, 3, 3, QL, QR);
+
+    // Zero below subdiagonal: W[2][0], W[3][0], W[4][0], W[3][1], W[4][1]
+    REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+    REQUIRE_THAT(W.Get(3, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+    REQUIRE_THAT(W.Get(4, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+    REQUIRE_THAT(W.Get(3, 1), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+    REQUIRE_THAT(W.Get(4, 1), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+
+    // Verify orthogonality
+    REQUIRE(isOrthogonal5(QL));
+    REQUIRE(isOrthogonal5(QR));
+  }
+}
+
+// ============================================================================
+// TEST 15: Bidiagonalize (Phase 1) — Wide matrix (m < n)
+// ===========================================================================
+TEST_CASE("SVD Phase 1: Bidiagonalize wide matrix", "[Matrix][SVD]") {
+  // Test case 1: 2×4 matrix
+  // C++ verified reference:
+  //   W[0] = [-5.099020, -6.275717, 11.401754,  0.000000]
+  //   W[1] = [ 0.000000, -0.784465,  2.806586, -0.350823]
+  // Note: W[1][2]=2.806586 and W[1][3]=-0.350823 are NOT zeroed because
+  // right HH at k=0 has only 2 elements (cols 2,3), so it zeros col 3 but preserves col 2
+  {
+    Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 4.0f, 0.0f,
+                   5.0f, 6.0f, 7.0f, 8.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 2, 4, 2, QL, QR);
+
+    // For 2×4: right HH at k=0 has 2 elements (cols 2,3)
+    // It zeros col 3 but preserves col 2 as the superdiagonal element for row 1
+    // W[0][3] should be zeroed (above superdiagonal in row 0)
+    REQUIRE_THAT(W.Get(0, 3), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+    // W[1][2] and W[1][3] are part of the bidiagonal structure for row 1
+    // (superdiagonal at col 2, and right HH preserves first element)
+    REQUIRE_THAT(W.Get(1, 2), !Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+
+    // Verify orthogonality
+    REQUIRE(isOrthogonal5(QL));
+    REQUIRE(isOrthogonal5(QR));
+  }
+
+  // Test case 2: 3×5 matrix
+  {
+    Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 4.0f, 5.0f,
+                   6.0f, 7.0f, 8.0f, 9.0f, 10.0f,
+                   0.0f, 11.0f, 12.0f, 13.0f, 14.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 3, 5, 3, QL, QR);
+
+    // For 3×5: check that subdiagonal elements are zero
+    REQUIRE_THAT(W.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+    REQUIRE_THAT(W.Get(2, 1), Catch::Matchers::WithinAbs(0.0f, 1e-4f));
+
+    // Verify orthogonality
+    REQUIRE(isOrthogonal5(QL));
+    REQUIRE(isOrthogonal5(QR));
+  }
+
+  // Test case 3: 1×3 matrix (row vector)
+  // C++ verified reference: W[0] = [1.0, 2.0, 3.0] (no transformations needed)
+  {
+    Matrix<5, 5> W{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 1, 3, 1, QL, QR);
+
+    // For 1×3, no transformations needed
+    REQUIRE_THAT(W.Get(0, 0), Catch::Matchers::WithinAbs(1.0f, 1e-4f));
+    REQUIRE_THAT(W.Get(0, 1), Catch::Matchers::WithinAbs(2.0f, 1e-4f));
+    REQUIRE_THAT(W.Get(0, 2), Catch::Matchers::WithinAbs(3.0f, 1e-4f));
+
+    // Verify orthogonality
+    REQUIRE(isOrthogonal5(QL));
+    REQUIRE(isOrthogonal5(QR));
+  }
+}
+
+// ============================================================================
+// TEST 16: Bidiagonalize — Reconstruction property
+// ===========================================================================
+TEST_CASE("SVD Phase 1: Bidiagonalize reconstruction property", "[Matrix][SVD]") {
+  // Test: QLᵀ · W_original · QR = B (bidiagonal)
+  // This verifies that the accumulated transformations correctly represent
+  // the bidiagonalization.
+  {
+    Matrix<5, 5> W_orig{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
+                        4.0f, 5.0f, 6.0f, 0.0f, 0.0f,
+                        0.0f, 7.0f, 8.0f, 0.0f, 0.0f,
+                        0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                        0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> W = W_orig;
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 3, 3, 3, QL, QR);
+
+    // Compute QLᵀ · W_orig · QR and verify it equals W (the bidiagonal result)
+    Matrix<5, 5> Qt = QL.Transpose();
+    Matrix<5, 5> QtW_orig{0};
+    Qt.Mult(W_orig, QtW_orig);
+
+    Matrix<5, 5> QtW_origQR{0};
+    QtW_orig.Mult(QR, QtW_origQR);
+
+    // The reconstruction should match the bidiagonal result
+    float err = frobeniusNorm5(W - QtW_origQR);
+    REQUIRE_THAT(err, Catch::Matchers::WithinAbs(1e-3f, 1e-3f));
+  }
+
+  // Test: Tall matrix reconstruction (4×3)
+  {
+    Matrix<5, 5> W_orig{1.0f, 2.0f, 3.0f, 0.0f, 0.0f,
+                        4.0f, 5.0f, 6.0f, 0.0f, 0.0f,
+                        0.0f, 7.0f, 8.0f, 0.0f, 0.0f,
+                        0.0f, 10.0f, 9.0f, 0.0f, 0.0f,
+                        0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> W = W_orig;
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 4, 3, 3, QL, QR);
+
+    Matrix<5, 5> Qt = QL.Transpose();
+    Matrix<5, 5> QtW_orig{0};
+    Qt.Mult(W_orig, QtW_orig);
+
+    Matrix<5, 5> QtW_origQR{0};
+    QtW_orig.Mult(QR, QtW_origQR);
+
+    float err = frobeniusNorm5(W - QtW_origQR);
+    REQUIRE_THAT(err, Catch::Matchers::WithinAbs(1e-3f, 1e-3f));
+  }
+
+  // Test: Wide matrix reconstruction (2×4)
+  {
+    Matrix<5, 5> W_orig{1.0f, 2.0f, 3.0f, 4.0f, 0.0f,
+                        5.0f, 6.0f, 7.0f, 8.0f, 0.0f,
+                        0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                        0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                        0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+    Matrix<5, 5> W = W_orig;
+
+    Matrix<5, 5> QL{0}, QR{0};
+    for (uint8_t i = 0; i < 5; i++) {
+      QL[i][i] = 1.0f;
+      QR[i][i] = 1.0f;
+    }
+
+    SVD::Bidiagonalize(W, 2, 4, 2, QL, QR);
+
+    Matrix<5, 5> Qt = QL.Transpose();
+    Matrix<5, 5> QtW_orig{0};
+    Qt.Mult(W_orig, QtW_orig);
+
+    Matrix<5, 5> QtW_origQR{0};
+    QtW_orig.Mult(QR, QtW_origQR);
+
+    float err = frobeniusNorm5(W - QtW_origQR);
+    REQUIRE_THAT(err, Catch::Matchers::WithinAbs(1e-3f, 1e-3f));
+  }
+}

unit-tests/svd-integration-test.cpp

@@ -62,7 +62,7 @@ TEST_CASE("SVD Integration: 2x2 [[1,2],[3,4]]", "[Matrix][SVD][Integration]") {
       err += diff * diff;
     }
   err = sqrtf(err);
-  REQUIRE_THAT(err, Catch::Matchers::WithinRel(0.0f, 1e-3f));
+  REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
 
   std::cout << "SVD 2x2 [[1,2],[3,4]]:\n";
   std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0)
@@ -86,8 +86,8 @@ TEST_CASE("SVD Integration: 3x3 diagonal [10,5,2]",
   // U and Vt should be identity (or close) for diagonal matrix
   float uErr = frobeniusNorm(U - Matrix<3, 3>{1, 0, 0, 0, 1, 0, 0, 0, 1});
   float vtErr = frobeniusNorm(Vt - Matrix<3, 3>{1, 0, 0, 0, 1, 0, 0, 0, 1});
-  REQUIRE_THAT(uErr, Catch::Matchers::WithinRel(0.0f, 1e-2f));
-  REQUIRE_THAT(vtErr, Catch::Matchers::WithinRel(0.0f, 1e-2f));
+  REQUIRE_THAT(uErr, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
+  REQUIRE_THAT(vtErr, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
 }
 
 TEST_CASE("SVD Integration: 3x3 rank-deficient [[1,2,3],[4,5,6],[7,8,9]]",
@@ -120,7 +120,7 @@ TEST_CASE("SVD Integration: 3x3 rank-deficient [[1,2,3],[4,5,6],[7,8,9]]",
       err += diff * diff;
     }
   err = sqrtf(err);
-  REQUIRE_THAT(err, Catch::Matchers::WithinRel(0.0f, 1e-2f));
+  REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
 
   std::cout << "SVD 3x3 rank-deficient:\n";
   std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0) << ", "
@@ -155,7 +155,7 @@ TEST_CASE("SVD Integration: tall 4x3 matrix", "[Matrix][SVD][Integration]") {
       err += diff * diff;
     }
   err = sqrtf(err);
-  REQUIRE_THAT(err, Catch::Matchers::WithinRel(0.0f, 1e-2f));
+  REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
 
   std::cout << "SVD tall 4x3:\n";
   std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0) << ", "
@@ -175,42 +175,25 @@ TEST_CASE("SVD Integration: wide 3x5 matrix", "[Matrix][SVD][Integration]") {
   REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(2.46540f, 1e-2f));
   REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-2f));
 
-  // Check reconstruction: U (3x5) * diag(sigma) (5x3) = 3x3, then * Vt (3x5) =
-  // 3x5
-  Matrix<3, 5> recon{0};
-  Matrix<3, 5> Usig{0};
-  for (int i = 0; i < 3; i++)
-    for (int j = 0; j < 5; j++)
-      Usig[i][j] = U.Get(i, j) * sigma.Get(j, 0);
-
-  // For wide matrix: A = U * Sigma * Vt where U is 3x5, Sigma is 5x5
-  // (diagonal), Vt is 5x5 But our implementation returns sigma as 3x1 and Vt as
-  // 3x5 So we need: recon = Usig (3x5) * Vt (3x5)^T ... no that doesn't work
-  // either The SVD for wide matrices is: A = U * Sigma * Vt where:
-  //   U is m×m (3×3), Sigma is m×n (3×5), Vt is n×n (5×5)
-  // But our API returns U as m×n (3×5), sigma as n×1 (3×1), Vt as n×n (3×5)
-  // So: recon = U (3x5) * diag(sigma) (5x5) * Vt (5x5)^T ...
-  // Actually, looking at the implementation, for wide matrices we swap roles.
-  // Let me just check reconstruction using the actual dimensions returned.
-
-  // For wide matrix: A (3x5) = U (3x5) * diag(sigma) (5x5 padded) * Vt (5x5)
-  // But our API returns Vt as 3x5, not 5x5
-  // The implementation stores: U = QR[:,0:p]^T (3x5), sigma (3x1), Vt =
-  // QL[:,0:p]^T (3x5) Reconstruction: A[i][j] = sum_k U[i][k]*sigma[k]*Vt[j][k]
+  // Check reconstruction: A (3x5) = U * Sigma * Vt, where U (3x5) has
+  // its meaningful part in the first 3 columns, sigma (5x1) in the
+  // first 3 entries, and Vt (5x5) in its first 3 rows (right
+  // singular vectors as rows). So:
+  //   A[i][j] = sum_k U[i][k] * sigma[k] * Vt[k][j]
 
   float err2 = 0.0f;
   for (int i = 0; i < 3; i++) {
     for (int j = 0; j < 5; j++) {
       float recon_val = 0.0f;
       for (int k = 0; k < 3; k++) {
-        recon_val += U.Get(i, k) * sigma.Get(k, 0) * Vt.Get(j, k);
+        recon_val += U.Get(i, k) * sigma.Get(k, 0) * Vt.Get(k, j);
       }
       float diff = recon_val - A.Get(i, j);
       err2 += diff * diff;
     }
   }
   err2 = sqrtf(err2);
-  REQUIRE_THAT(err2, Catch::Matchers::WithinRel(0.0f, 1e-2f));
+  REQUIRE_THAT(err2, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
 
   std::cout << "SVD wide 3x5:\n";
   std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0) << ", "
@@ -230,7 +213,7 @@ TEST_CASE("SVD Integration: identity 3x3", "[Matrix][SVD][Integration]") {
   REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(1.0f, 1e-3f));
 
   float err = frobeniusNorm(U - Matrix<3, 3>{1, 0, 0, 0, 1, 0, 0, 0, 1});
-  REQUIRE_THAT(err, Catch::Matchers::WithinRel(0.0f, 1e-2f));
+  REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
 }
 
 TEST_CASE("SVD Integration: symmetric positive definite 2x2 [[5,3],[3,5]]",
@@ -261,7 +244,7 @@ TEST_CASE("SVD Integration: symmetric positive definite 2x2 [[5,3],[3,5]]",
       err += diff * diff;
     }
   err = sqrtf(err);
-  REQUIRE_THAT(err, Catch::Matchers::WithinRel(0.0f, 1e-3f));
+  REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
 
   std::cout << "SVD SPD 2x2 [[5,3],[3,5]]:\n";
   std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0)