src/SVD.cpp

@@ -150,6 +150,103 @@ void SVD::ApplyGivensRight(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c,
   }
 }
 
+// ============================================================================
+// Phase 3: Extract and Sort Singular Values
+// ============================================================================
+
+void SVD::ExtractAndSortSingularValues(Matrix<5, 5> &W,
+                                        Matrix<5, 1> &sigma,
+                                        uint8_t p,
+                                        Matrix<5, 5> &QL,
+                                        Matrix<5, 5> &QR) {
+  // Extract singular values as absolute values of diagonal elements
+  for (uint8_t i = 0; i < p; i++) {
+    sigma[i][0] = fabsf(W[i][i]);
+  }
+
+  // Sort singular values in descending order and reorder U, V accordingly
+  for (uint8_t i = 0; i < p - 1; i++) {
+    for (uint8_t j = i + 1; j < p; j++) {
+      if (sigma[j][0] > sigma[i][0]) {
+        // Swap singular values
+        float tmpS = sigma[i][0];
+        sigma[i][0] = sigma[j][0];
+        sigma[j][0] = tmpS;
+
+        // Swap columns of QL
+        for (uint8_t k = 0; k < 5; k++) {
+          float tmpQ = QL[k][i];
+          QL[k][i] = QL[k][j];
+          QL[k][j] = tmpQ;
+        }
+
+        // Swap columns of QR
+        for (uint8_t k = 0; k < 5; k++) {
+          float tmpQ = QR[k][i];
+          QR[k][i] = QR[k][j];
+          QR[k][j] = tmpQ;
+        }
+      }
+    }
+  }
+}
+
+// ============================================================================
+// Phase 4: Assemble Final U and Vt Matrices
+// ============================================================================
+
+void SVD::AssembleUAndVt(uint8_t m, uint8_t n, uint8_t p,
+                         bool transposeNeeded,
+                         const Matrix<5, 5> &QL,
+                         const Matrix<5, 5> &QR,
+                         Matrix<5, 5> &U,
+                         Matrix<5, 5> &Vt) {
+  // Initialize output matrices to zero
+  for (uint8_t i = 0; i < 5; i++)
+    for (uint8_t j = 0; j < 5; j++) {
+      U[i][j] = 0;
+      Vt[i][j] = 0;
+    }
+
+  // ---- 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]ᵀ
+
+  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);
+        } else {
+          // U = QL[:, 0:p]
+          U[i][j] = QL.Get(i, j);
+        }
+      } else {
+        U[i][j] = 0;
+      }
+    }
+  }
+
+  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);
+        }
+      } else {
+        Vt[i][j] = 0;
+      }
+    }
+  }
+}
+
 // ============================================================================
 // SVD Implementation - Golub-Kahan-Reinsch Algorithm
 // ============================================================================
@@ -224,122 +321,45 @@ void SVD::SVD(Matrix<rows, columns> &matrixToDecompose,
         x[i] = W[k + i][k];
       }
 
-      // Compute Householder reflection: H·x = [α, 0, ..., 0]ᵀ
+      // Use building block to compute Householder reflector
-      float norm = 0;
+      float alpha;
-      for (uint8_t i = 0; i < len; i++)
+      SVD::ComputeHouseholder(x, len, hhVec, alpha);
-        norm += x[i] * x[i];
-      norm = sqrtf(norm);
 
-      if (norm < 1e-30f)
+      if (alpha == 0.0f)
         continue;
 
-      float alpha = (x[0] >= 0) ? -norm : norm;
-      float v0 = x[0] - alpha;
-      float vv = v0 * v0 + norm * norm - alpha * x[0];
-      if (vv < 1e-30f)
-        continue;
-
-      float scale = 1.0f / sqrtf(vv);
-      // Store Householder vector (first element is implicit 1, rest in hhVec)
-      hhVec[0] =
-          v0 * scale; // this is the first element of the reflected vector
-      for (uint8_t i = 1; i < len; i++) {
-        hhVec[i] = x[i] * scale;
-      }
-
       // Apply H from left to W: W = H·W
-      // For each column j, w[k+i][j] -= 2·v_i·(vᵀ·w_col) / (vᵀv)
+      SVD::ApplyHouseholderLeft(W, hhVec, k, k + len - 1);
-      float vvNorm = 1.0f + hhVec[0] * hhVec[0];
-      for (uint8_t i = 1; i < len; i++) {
-        vvNorm += hhVec[i] * hhVec[i];
-      }
-
-      for (uint8_t j = k; j < n; j++) {
-        float dot = 0;
-        for (uint8_t i = 0; i < len; i++) {
-          dot += hhVec[i] * W[k + i][j];
-        }
-        dot *= 2.0f / vvNorm;
-        for (uint8_t i = 0; i < len; i++) {
-          W[k + i][j] -= dot * hhVec[i];
-        }
-      }
 
       // Apply H from right to QL: QL = QL · H
-      for (uint8_t j = k; j < m; j++) {
+      SVD::ApplyHouseholderRight(QL, hhVec, k, k + len - 1);
-        float dot = 0;
-        for (uint8_t i = 0; i < len; i++) {
-          dot += hhVec[i] * QL[j][k + i];
-        }
-        dot *= 2.0f / vvNorm;
-        for (uint8_t i = 0; i < len; i++) {
-          QL[j][k + i] -= dot * hhVec[i];
-        }
-      }
     }
 
-    // --- Right Householder on row k, columns k+1..min(m,n)-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 - 1 - k : 0;
+      uint8_t len = (p > 1) ? p - 2 - k : 0;
       if (len <= 0)
         continue;
 
-      // Extract the row segment
+      // 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 + 1 + i];
+        x[i] = W[k][k + 2 + i];
       }
 
-      // Compute Householder reflection
+      // Use building block to compute Householder reflector
-      float norm = 0;
+      float alpha;
-      for (uint8_t i = 0; i < len; i++)
+      SVD::ComputeHouseholder(x, len, hhVec, alpha);
-        norm += x[i] * x[i];
-      norm = sqrtf(norm);
 
-      if (norm < 1e-30f)
+      if (alpha == 0.0f)
         continue;
 
-      float alpha = (x[0] >= 0) ? -norm : norm;
-      float v0 = x[0] - alpha;
-      float vv = v0 * v0 + norm * norm - alpha * x[0];
-      if (vv < 1e-30f)
-        continue;
-
-      float scale = 1.0f / sqrtf(vv);
-      hhVec[0] = v0 * scale;
-      for (uint8_t i = 1; i < len; i++) {
-        hhVec[i] = x[i] * scale;
-      }
-
-      // Compute vᵀv
-      float vvNorm = 1.0f + hhVec[0] * hhVec[0];
-      for (uint8_t i = 1; i < len; i++) {
-        vvNorm += hhVec[i] * hhVec[i];
-      }
-
       // Apply H from right to W: W = W·H
-      for (uint8_t i = 0; i < m; i++) {
+      SVD::ApplyHouseholderRight(W, hhVec, k + 2, k + 1 + len);
-        float dot = 0;
-        for (uint8_t j = 0; j < len; j++) {
-          dot += hhVec[j] * W[i][k + 1 + j];
-        }
-        dot *= 2.0f / vvNorm;
-        for (uint8_t j = 0; j < len; j++) {
-          W[i][k + 1 + j] -= dot * hhVec[j];
-        }
-      }
 
       // Apply H from right to QR: QR = QR · H
-      for (uint8_t i = 0; i < n; i++) {
+      SVD::ApplyHouseholderRight(QR, hhVec, k + 2, k + 1 + len);
-        float dot = 0;
-        for (uint8_t j = 0; j < len; j++) {
-          dot += hhVec[j] * QR[i][k + 1 + j];
-        }
-        dot *= 2.0f / vvNorm;
-        for (uint8_t j = 0; j < len; j++) {
-          QR[i][k + 1 + j] -= dot * hhVec[j];
-        }
-      }
     }
   }
 
@@ -425,32 +445,15 @@ void SVD::SVD(Matrix<rows, columns> &matrixToDecompose,
     float y = (start > 0) ? W[start][start - 1] : 0.0f;
 
     for (uint8_t i = start; i <= end; i++) {
-      float r = sqrtf(x * x + y * y);
+      float cs, sn;
-      if (r < 1e-30f) {
+      SVD::ComputeGivens(x, y, cs, sn);
-        x = W[i][i];
-        y = (i < end) ? W[i + 1][i] : 0.0f;
-        continue;
-      }
-
-      float cs = x / r;
-      float sn = y / r;
 
       // Apply Givens from left to rows i, i+1 of W (columns i..p-1)
-      for (uint8_t j = i; j < p; j++) {
+      SVD::ApplyGivensLeft(W, i, i + 1, cs, sn, i, p - 1);
-        float t1 = W[i][j];
-        float t2 = W[i + 1][j];
-        W[i][j] = cs * t1 + sn * t2;
-        W[i + 1][j] = -sn * t1 + cs * t2;
-      }
 
       // Apply Givens from right to columns i, i+1 of W (rows 0..i)
       if (i > start) {
-        for (uint8_t j = 0; j <= i; j++) {
+        SVD::ApplyGivensRight(W, i, i + 1, cs, sn, 0, i);
-          float t1 = W[j][i];
-          float t2 = W[j][i + 1];
-          W[j][i] = cs * t1 + sn * t2;
-          W[j][i + 1] = -sn * t1 + cs * t2;
-        }
       }
 
       // Accumulate right transformations into QR
@@ -462,77 +465,35 @@ void SVD::SVD(Matrix<rows, columns> &matrixToDecompose,
       }
 
       // Prepare next Givens rotation
-      x = W[i + 1][i];
+      if (i + 1 <= end) {
-      y = (i + 1 < end) ? W[i + 1][i + 1] : 0.0f;
+        x = W[i + 1][i];
-    }
+        y = (i + 1 < end) ? W[i + 1][i + 1] : 0.0f;
-  }
-
-  // ---- Phase 3: Extract Results ----
-  // Singular values are the absolute values of diagonal elements of W
-  for (uint8_t i = 0; i < p; i++) {
-    sigma[i][0] = fabsf(W[i][i]);
-  }
-
-  // Sort singular values in descending order and reorder U, V accordingly
-  for (uint8_t i = 0; i < p - 1; i++) {
-    for (uint8_t j = i + 1; j < p; j++) {
-      if (sigma[j][0] > sigma[i][0]) {
-        float tmpS = sigma[i][0];
-        sigma[i][0] = sigma[j][0];
-        sigma[j][0] = tmpS;
-
-        // Swap columns of QL
-        for (uint8_t k = 0; k < 5; k++) {
-          float tmpQ = QL[k][i];
-          QL[k][i] = QL[k][j];
-          QL[k][j] = tmpQ;
-        }
-
-        // Swap columns of QR
-        for (uint8_t k = 0; k < 5; k++) {
-          float tmpQ = QR[k][i];
-          QR[k][i] = QR[k][j];
-          QR[k][j] = tmpQ;
-        }
       }
     }
   }
 
-  // ---- Phase 4: Compute Final U and Vt ----
+  // ---- Phase 3: Extract and Sort Singular Values ----
-  // If transposeNeeded (wide matrix), we computed SVD(Aᵀ) = Ũ·Σ·Ṽᵀ
+  // Use internal 5×1 buffer for sigma
-  //   Then U = Ṽ (= QR[:,0:p]) and Vt = Ũᵀ (= QL[:,0:p]ᵀ)
+  Matrix<5, 1> sigmaInternal{0};
-  // Otherwise, SVD(A) = QL[:,0:p] · Σ · (QR[:,0:p])ᵀ
+  ExtractAndSortSingularValues(W, sigmaInternal, p, QL, QR);
-  //   So U = QL[:,0:p] and Vt = QR[:,0:p]ᵀ
 
-  for (uint8_t i = 0; i < m; i++) {
+  // ---- Phase 4: Assemble Final U and Vt ----
-    for (uint8_t j = 0; j < n; j++) {
+  // Use internal 5×5 buffers for U and Vt
-      if (j < p) {
+  Matrix<5, 5> UInternal{0}, VtInternal{0};
-        if (transposeNeeded) {
+  AssembleUAndVt(m, n, p, transposeNeeded, QL, QR, UInternal, VtInternal);
-          // U = QR[:, 0:p]ᵀ → U[i][j] = QR[j][i]
+
-          U[i][j] = QR[j][i];
+  // Copy results to output parameters
-        } else {
+  for (uint8_t i = 0; i < columns; i++) {
-          // U = QL[:, 0:p]
+    sigma[i][0] = sigmaInternal.Get(i, 0);
-          U[i][j] = QL[i][j];
+  }
-        }
+  for (uint8_t i = 0; i < rows; i++) {
-      } else {
+    for (uint8_t j = 0; j < columns; j++) {
-        U[i][j] = 0;
+      U[i][j] = UInternal.Get(i, j);
-      }
     }
   }
-
+  for (uint8_t i = 0; i < columns; i++) {
-  for (uint8_t i = 0; i < n; i++) {
+    for (uint8_t j = 0; j < rows; j++) {
-    for (uint8_t j = 0; j < m; j++) {
+      Vt[i][j] = VtInternal.Get(i, j);
-      if (i < p && j < m) {
-        if (transposeNeeded) {
-          // Vt = QL[:, 0:p]ᵀ → Vt[i][j] = QL[j][i]
-          Vt[i][j] = QL[j][i];
-        } else {
-          // Vt = QR[:, 0:p]ᵀ → Vt[i][j] = QR[j][i]
-          Vt[i][j] = QR[j][i];
-        }
-      } else {
-        Vt[i][j] = 0;
-      }
     }
   }
 }

src/SVD.hpp

@@ -78,6 +78,50 @@ 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 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.
+ *
+ * @param W Input: bidiagonal matrix (5×5 working array)
+ * @param sigma Output: sorted singular values (5×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)
+ */
+static void ExtractAndSortSingularValues(Matrix<5, 5> &W,
+                                          Matrix<5, 1> &sigma,
+                                          uint8_t p,
+                                          Matrix<5, 5> &QL,
+                                          Matrix<5, 5> &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 = QL[:,0:p]ᵀ
+ *
+ * @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 (5×5)
+ * @param QR Right Householder accumulation (5×5)
+ * @param U Output: left singular vectors (m×n matrix, only first p columns used)
+ * @param Vt Output: right singular vectors transposed (n×n matrix, only first p rows used)
+ */
+static void AssembleUAndVt(uint8_t m, uint8_t n, uint8_t p,
+                           bool transposeNeeded,
+                           const Matrix<5, 5> &QL,
+                           const Matrix<5, 5> &QR,
+                           Matrix<5, 5> &U,
+                           Matrix<5, 5> &Vt);
+
 /**
  * @brief Compute a Givens rotation that zeros out y.
  *

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

@@ -783,3 +783,245 @@ TEST_CASE("SVD Building Block: Givens preserves orthogonality",
     REQUIRE(isOrthogonal5(M_right));
   }
 }
+
+// ============================================================================
+// TEST 11: ExtractAndSortSingularValues (Phase 3)
+// ===========================================================================
+TEST_CASE("SVD Phase 3: ExtractAndSortSingularValues", "[Matrix][SVD]") {
+  // Test case 1: Diagonal matrix with unordered singular values
+  {
+    Matrix<5, 5> W{0};
+    W[0][0] = 2.0f;
+    W[1][1] = 10.0f;
+    W[2][2] = 5.0f;
+
+    Matrix<5, 1> sigma{0};
+    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::ExtractAndSortSingularValues(W, sigma, 3, QL, QR);
+
+    // Singular values should be sorted descending: [10, 5, 2]
+    REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(10.0f, 1e-6f));
+    REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(5.0f, 1e-6f));
+    REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(2.0f, 1e-6f));
+
+    // QL and QR columns should have been swapped to match the sort order
+    // Original: col 0 → σ=2, col 1 → σ=10, col 2 → σ=5
+    // After sort: col 0 has σ=10 (was orig col 1), col 1 has σ=5 (was orig col 2),
+    //             col 2 has σ=2 (was orig col 0)
+    // Starting from identity: QL[:,0] = e₀, QL[:,1] = e₁, QL[:,2] = e₂
+    // After swaps: QL[:,0] = e₁, QL[:,1] = e₂, QL[:,2] = e₀
+    REQUIRE_THAT(QL.Get(0, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(QL.Get(1, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+    REQUIRE_THAT(QL.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(QL.Get(0, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(QL.Get(1, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(QL.Get(2, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+    REQUIRE_THAT(QL.Get(0, 2), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+    REQUIRE_THAT(QL.Get(1, 2), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(QL.Get(2, 2), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+  }
+
+  // Test case 2: Negative diagonal elements (absolute value extraction)
+  {
+    Matrix<5, 5> W{0};
+    W[0][0] = -3.0f;
+    W[1][1] = -7.0f;
+    W[2][2] = 5.0f;
+
+    Matrix<5, 1> sigma{0};
+    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::ExtractAndSortSingularValues(W, sigma, 3, QL, QR);
+
+    // Should extract absolute values and sort: [7, 5, 3]
+    REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(7.0f, 1e-6f));
+    REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(5.0f, 1e-6f));
+    REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(3.0f, 1e-6f));
+  }
+
+  // Test case 3: Already sorted (no swaps needed)
+  {
+    Matrix<5, 5> W{0};
+    W[0][0] = 9.0f;
+    W[1][1] = 6.0f;
+    W[2][2] = 3.0f;
+
+    Matrix<5, 1> sigma{0};
+    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::ExtractAndSortSingularValues(W, sigma, 3, QL, QR);
+
+    REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(9.0f, 1e-6f));
+    REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(6.0f, 1e-6f));
+    REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(3.0f, 1e-6f));
+
+    // QL and QR should be unchanged (identity)
+    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(QL.Get(i, j), Catch::Matchers::WithinRel(expected, 1e-6f));
+        REQUIRE_THAT(QR.Get(i, j), Catch::Matchers::WithinRel(expected, 1e-6f));
+      }
+    }
+  }
+
+  // Test case 4: Single singular value
+  {
+    Matrix<5, 5> W{0};
+    W[0][0] = 42.0f;
+
+    Matrix<5, 1> sigma{0};
+    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::ExtractAndSortSingularValues(W, sigma, 1, QL, QR);
+
+    REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(42.0f, 1e-6f));
+  }
+}
+
+// ============================================================================
+// TEST 12: AssembleUAndVt (Phase 4)
+// ===========================================================================
+TEST_CASE("SVD Phase 4: AssembleUAndVt", "[Matrix][SVD]") {
+  // Test case 1: Non-transpose case (m ≥ n) — U from QL, Vt from QRᵀ
+  {
+    uint8_t m = 3, n = 2, p = 2;
+    bool transposeNeeded = false;
+
+    Matrix<5, 5> QL{0};
+    // Make columns orthonormal
+    QL[0][0] = 3.0f / 5.0f;
+    QL[1][0] = 4.0f / 5.0f;
+    QL[2][0] = 0.0f;
+    QL[0][1] = 4.0f / 5.0f;
+    QL[1][1] = -3.0f / 5.0f;
+    QL[2][1] = 0.0f;
+
+    Matrix<5, 5> QR{0};
+    QR[0][0] = 1.0f;   // Vt[:,0]ᵀ
+    QR[1][0] = 0.0f;
+    QR[0][1] = 0.0f;   // Vt[:,1]ᵀ
+    QR[1][1] = 1.0f;
+
+    Matrix<5, 5> U{0};
+    Matrix<5, 5> Vt{0};
+
+    SVD::AssembleUAndVt(m, n, p, transposeNeeded, QL, QR, U, Vt);
+
+    // U should be QL[:,0:2]
+    REQUIRE_THAT(U.Get(0, 0), Catch::Matchers::WithinRel(3.0f / 5.0f, 1e-6f));
+    REQUIRE_THAT(U.Get(1, 0), Catch::Matchers::WithinRel(4.0f / 5.0f, 1e-6f));
+    REQUIRE_THAT(U.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(U.Get(0, 1), Catch::Matchers::WithinRel(4.0f / 5.0f, 1e-6f));
+    REQUIRE_THAT(U.Get(1, 1), Catch::Matchers::WithinRel(-3.0f / 5.0f, 1e-6f));
+    REQUIRE_THAT(U.Get(2, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+
+    // Vt should be QR[:,0:2]ᵀ
+    REQUIRE_THAT(Vt.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+    REQUIRE_THAT(Vt.Get(0, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(Vt.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(Vt.Get(1, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+
+    // Verify U is orthogonal (first p columns)
+    Matrix<5, 5> Ut = U.Transpose();
+    Matrix<5, 5> UtU{0};
+    Ut.Mult(U, UtU);
+    REQUIRE_THAT(UtU.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+    REQUIRE_THAT(UtU.Get(0, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(UtU.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(UtU.Get(1, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+  }
+
+  // Test case 2: Transpose case (m < n) — U from QRᵀ, Vt from QLᵀ
+  {
+    uint8_t m = 2, n = 3, p = 2;
+    bool transposeNeeded = true;
+
+    Matrix<5, 5> QL{0};
+    QL[0][0] = 1.0f;   // Vt[:,0]ᵀ
+    QL[1][0] = 0.0f;
+    QL[0][1] = 0.0f;   // Vt[:,1]ᵀ
+    QL[1][1] = 1.0f;
+
+    Matrix<5, 5> QR{0};
+    QR[0][0] = 3.0f / 5.0f;   // U[:,0]
+    QR[1][0] = 4.0f / 5.0f;
+    QR[2][0] = 0.0f;
+    QR[0][1] = 4.0f / 5.0f;
+    QR[1][1] = -3.0f / 5.0f;
+    QR[2][1] = 0.0f;
+
+    Matrix<5, 5> U{0};
+    Matrix<5, 5> Vt{0};
+
+    SVD::AssembleUAndVt(m, n, p, transposeNeeded, QL, QR, U, Vt);
+
+    // U should be QR[:,0:2]ᵀ → U[i][j] = QR[j][i]
+    REQUIRE_THAT(U.Get(0, 0), Catch::Matchers::WithinRel(3.0f / 5.0f, 1e-6f));
+    REQUIRE_THAT(U.Get(0, 1), Catch::Matchers::WithinRel(4.0f / 5.0f, 1e-6f));
+    REQUIRE_THAT(U.Get(1, 0), Catch::Matchers::WithinRel(4.0f / 5.0f, 1e-6f));
+    REQUIRE_THAT(U.Get(1, 1), Catch::Matchers::WithinRel(-3.0f / 5.0f, 1e-6f));
+
+    // Vt should be QL[:,0:2]ᵀ → Vt[i][j] = QL[j][i]
+    REQUIRE_THAT(Vt.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+    REQUIRE_THAT(Vt.Get(0, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(Vt.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
+    REQUIRE_THAT(Vt.Get(1, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+
+    // Verify U has correct dimensions (m×n = 2×3)
+    REQUIRE(U.Get(0, 2) == 0.0f);
+    REQUIRE(U.Get(1, 2) == 0.0f);
+
+    // Verify Vt is orthogonal (first p rows)
+    Matrix<5, 5> VtVtT{0};
+    Vt.Mult(Vt.Transpose(), VtVtT);
+    REQUIRE_THAT(VtVtT.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+    REQUIRE_THAT(VtVtT.Get(1, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+    // Row 2 of Vt is all zeros (p=2 < n=3), so VtVtT[2][2] = 0 is expected
+  }
+
+  // Test case 3: Square matrix (m = n)
+  {
+    uint8_t m = 2, n = 2, p = 2;
+    bool transposeNeeded = false;
+
+    Matrix<5, 5> QL{0};
+    QL[0][0] = 1.0f; QL[1][1] = 1.0f;
+
+    Matrix<5, 5> QR{0};
+    QR[0][0] = 0.6f; QR[0][1] = 0.8f;
+    QR[1][0] = 0.8f; QR[1][1] = -0.6f;
+
+    Matrix<5, 5> U{0};
+    Matrix<5, 5> Vt{0};
+
+    SVD::AssembleUAndVt(m, n, p, transposeNeeded, QL, QR, U, Vt);
+
+    // U = QL[:,0:2]
+    REQUIRE_THAT(U.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+    REQUIRE_THAT(U.Get(1, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+
+    // Vt = QR[:,0:2]ᵀ
+    REQUIRE_THAT(Vt.Get(0, 0), Catch::Matchers::WithinRel(0.6f, 1e-6f));
+    REQUIRE_THAT(Vt.Get(0, 1), Catch::Matchers::WithinRel(0.8f, 1e-6f));
+    REQUIRE_THAT(Vt.Get(1, 0), Catch::Matchers::WithinRel(0.8f, 1e-6f));
+    REQUIRE_THAT(Vt.Get(1, 1), Catch::Matchers::WithinRel(-0.6f, 1e-6f));
+  }
+}