Function lupnt::UDUDecomposition

• Defined in File udu_utils.cc

Function Documentation

VecMatPair lupnt::UDUDecomposition(const MatXd &P)

Factor a symmetric positive-(semi)definite covariance matrix as P = U diag(D) U^T, with U unit upper-triangular and D diagonal.

Used by UDUEKF::SetUd to (re)factor the filter covariance after initialization and after each Predict step, enabling the square-root-style sequential CarlsonUpdate. Diagonal entries of D that are numerically zero (within 1e-16) are clamped to exactly zero.

Parameters:

P – Symmetric covariance matrix to factor, size [n x n]

Returns:

Pair (D, U): diagonal factor D (size [n]) and unit-upper-triangular factor U (size [n x n])