Fixes for control systems AND SVD and QR decomposition #9
+8
-2
@@ -21,7 +21,13 @@ namespace QR {
|
|||||||
* GivensRotation: R * (a, b)^T = (r, 0)^T with R = [[c, s], [-s, c]],
|
* GivensRotation: R * (a, b)^T = (r, 0)^T with R = [[c, s], [-s, c]],
|
||||||
* r = +hypot(a, b), c = a/r, s = b/r.
|
* r = +hypot(a, b), c = a/r, s = b/r.
|
||||||
*/
|
*/
|
||||||
static void GivensRotation(float a, float b, float &c, float &s) {
|
// [[maybe_unused]]: this helper is only referenced from template
|
||||||
|
// (EigenQR/Tridiagonalize), so in translation units that include this file
|
||||||
|
// but never instantiate those templates, the definition is legitimately
|
||||||
|
// unused. The attribute silences -Wunused-function there without hiding
|
||||||
|
// real dead code in TUs that do use the algorithm.
|
||||||
|
[[maybe_unused]] static void GivensRotation(float a, float b, float &c,
|
||||||
|
float &s) {
|
||||||
float r = sqrtf(a * a + b * b);
|
float r = sqrtf(a * a + b * b);
|
||||||
if (r == 0.0f) {
|
if (r == 0.0f) {
|
||||||
c = 1.0f;
|
c = 1.0f;
|
||||||
@@ -111,7 +117,7 @@ static void ApplyRotationToVectors(Matrix<N, N> &V, uint8_t i, float c,
|
|||||||
* WilkinsonShift: eigenvalue of [[a, b], [b, d]] closest to d.
|
* WilkinsonShift: eigenvalue of [[a, b], [b, d]] closest to d.
|
||||||
* mu = (a+d)/2 - sign(a-d) * sqrt(((a-d)/2)^2 + b^2), sign(0) = +1.
|
* mu = (a+d)/2 - sign(a-d) * sqrt(((a-d)/2)^2 + b^2), sign(0) = +1.
|
||||||
*/
|
*/
|
||||||
static float WilkinsonShift(float a, float b, float d) {
|
[[maybe_unused]] static float WilkinsonShift(float a, float b, float d) {
|
||||||
float delta = 0.5f * (a - d);
|
float delta = 0.5f * (a - d);
|
||||||
float spread = sqrtf(delta * delta + b * b);
|
float spread = sqrtf(delta * delta + b * b);
|
||||||
return 0.5f * (a + d) - (delta >= 0.0f ? spread : -spread);
|
return 0.5f * (a + d) - (delta >= 0.0f ? spread : -spread);
|
||||||
|
|||||||
-28
@@ -90,20 +90,6 @@ template <uint8_t N>
|
|||||||
void EigenQR(Matrix<N, N> &matrixToDecompose, Matrix<N, N> &eigenVectors,
|
void EigenQR(Matrix<N, N> &matrixToDecompose, Matrix<N, N> &eigenVectors,
|
||||||
Matrix<N, 1> &eigenValues, uint32_t maxIterations, float tolerance);
|
Matrix<N, 1> &eigenValues, uint32_t maxIterations, float tolerance);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Compute a Givens rotation that zeros the bottom entry of (a, b)
|
|
||||||
*
|
|
||||||
* Given the column vector (a, b), produces (c, s) defining the 2x2
|
|
||||||
* rotation
|
|
||||||
* R = [ c s ]
|
|
||||||
* [ -s c ]
|
|
||||||
* such that R * (a, b)^T = (r, 0)^T with r = +hypot(a, b) >= 0, i.e.
|
|
||||||
* c = a / r, s = b / r.
|
|
||||||
*
|
|
||||||
* If (a, b) == (0, 0) the identity rotation (c = 1, s = 0) is returned.
|
|
||||||
*/
|
|
||||||
static void GivensRotation(float a, float b, float &c, float &s);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Apply the similarity transform A <- G A G^T on rows/cols (i, i+1)
|
* @brief Apply the similarity transform A <- G A G^T on rows/cols (i, i+1)
|
||||||
*
|
*
|
||||||
@@ -143,20 +129,6 @@ template <uint8_t N>
|
|||||||
static void ApplyRotationToVectors(Matrix<N, N> &V, uint8_t i, float c,
|
static void ApplyRotationToVectors(Matrix<N, N> &V, uint8_t i, float c,
|
||||||
float s);
|
float s);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Wilkinson shift for a symmetric tridiagonal
|
|
||||||
*
|
|
||||||
* Given the trailing 2x2 block
|
|
||||||
* [ a b ]
|
|
||||||
* [ b d ]
|
|
||||||
* returns the eigenvalue of that block that is closest to d. This is the
|
|
||||||
* empirically best shift for the QR iteration (Trefethen & Bau 13.4.1).
|
|
||||||
*
|
|
||||||
* mu = (a+d)/2 - sign(a-d) * sqrt(((a-d)/2)^2 + b^2)
|
|
||||||
* (with sign(0) taken as +1).
|
|
||||||
*/
|
|
||||||
static float WilkinsonShift(float a, float b, float d);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Solve the 2x2 eigenproblem of block rows/cols (lo, lo+1)
|
* @brief Solve the 2x2 eigenproblem of block rows/cols (lo, lo+1)
|
||||||
*
|
*
|
||||||
|
|||||||
+4
-4
@@ -21,8 +21,8 @@
|
|||||||
// template parameter N (a compile-time constant per instantiation).
|
// template parameter N (a compile-time constant per instantiation).
|
||||||
// ============================================================================
|
// ============================================================================
|
||||||
|
|
||||||
float SVD::ComputeHouseholder(const float *x, uint8_t len, float *v,
|
[[maybe_unused]] float SVD::ComputeHouseholder(const float *x, uint8_t len,
|
||||||
float &alpha) {
|
float *v, float &alpha) {
|
||||||
// Compute ||x||
|
// Compute ||x||
|
||||||
float norm = 0.0f;
|
float norm = 0.0f;
|
||||||
for (uint8_t i = 0; i < len; i++) {
|
for (uint8_t i = 0; i < len; i++) {
|
||||||
@@ -506,7 +506,7 @@ template <uint8_t N>
|
|||||||
void SVD::SolveBidiagonalBlockJacobi(Matrix<N, N> &W, uint8_t blockStart,
|
void SVD::SolveBidiagonalBlockJacobi(Matrix<N, N> &W, uint8_t blockStart,
|
||||||
uint8_t blockSize, uint8_t rowsQL,
|
uint8_t blockSize, uint8_t rowsQL,
|
||||||
uint8_t rowsQR, Matrix<N, N> &QL,
|
uint8_t rowsQR, Matrix<N, N> &QL,
|
||||||
Matrix<N, N> &QR, float tol) {
|
Matrix<N, N> &QR) {
|
||||||
// Full SVD of an unreduced upper-bidiagonal block of size > 2, computed
|
// Full SVD of an unreduced upper-bidiagonal block of size > 2, computed
|
||||||
// as the eigen-decomposition of the symmetric tridiagonal T = BᵀB:
|
// as the eigen-decomposition of the symmetric tridiagonal T = BᵀB:
|
||||||
//
|
//
|
||||||
@@ -948,7 +948,7 @@ void SVD::SVD(Matrix<rows, columns> &matrixToDecompose,
|
|||||||
} else if (blockSize > 2) {
|
} else if (blockSize > 2) {
|
||||||
// Larger block: cyclic Jacobi eigen-solve of BᵀB
|
// Larger block: cyclic Jacobi eigen-solve of BᵀB
|
||||||
SVD::SolveBidiagonalBlockJacobi(W, blockStart, blockSize, m, n, QL,
|
SVD::SolveBidiagonalBlockJacobi(W, blockStart, blockSize, m, n, QL,
|
||||||
QR, 1e-10f);
|
QR);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Move to the next block
|
// Move to the next block
|
||||||
|
|||||||
+1
-2
@@ -302,13 +302,12 @@ static void ApplyBlockFactorsToAccumulators(uint8_t blockStart,
|
|||||||
* @param rowsQR Number of meaningful rows of QR
|
* @param rowsQR Number of meaningful rows of QR
|
||||||
* @param QL Input/output: left transformation accumulator
|
* @param QL Input/output: left transformation accumulator
|
||||||
* @param QR Input/output: right transformation accumulator
|
* @param QR Input/output: right transformation accumulator
|
||||||
* @param tol (unused: Jacobi convergence tolerance is internal)
|
|
||||||
*/
|
*/
|
||||||
template <uint8_t N>
|
template <uint8_t N>
|
||||||
static void SolveBidiagonalBlockJacobi(Matrix<N, N> &W, uint8_t blockStart,
|
static void SolveBidiagonalBlockJacobi(Matrix<N, N> &W, uint8_t blockStart,
|
||||||
uint8_t blockSize, uint8_t rowsQL,
|
uint8_t blockSize, uint8_t rowsQL,
|
||||||
uint8_t rowsQR, Matrix<N, N> &QL,
|
uint8_t rowsQR, Matrix<N, N> &QL,
|
||||||
Matrix<N, N> &QR, float tol);
|
Matrix<N, N> &QR);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Extract singular values from bidiagonal matrix diagonal and sort.
|
* @brief Extract singular values from bidiagonal matrix diagonal and sort.
|
||||||
|
|||||||
Reference in New Issue
Block a user