Function lupnt::ModifiedGramSchmidt¶

Defined in File udu_utils.h

Function Documentation¶

VecMatPair lupnt::ModifiedGramSchmidt(const MatXd &D_tilde, const MatXd &Y)¶

Modified weighted Gram-Schmidt (MWGS) orthogonalization for the numerically stable UDU time update (Thornton’s algorithm).

Given Y (size [n x (n+m)]) and a symmetric weight matrix D_tilde (size [(n+m) x (n+m)]), returns (D, U) — a diagonal D (size [n]) and a unit-upper-triangular U (size [n x n]) — such that U diag(D) U^T = Y D_tilde Y^T, computed without ever forming the product. For the UDU predict step, Y = [F U | G] and D_tilde = blkdiag(D_prev, Q), so this propagates the factored covariance and folds in the process noise in one stable pass — unlike reconstructing P, adding Q, and re-factoring, which loses variances many orders of magnitude below the largest one.

Parameters:

D_tilde – Symmetric weight matrix, size [(n+m) x (n+m)]
Y – Coefficient matrix, size [n x (n+m)]

Returns:

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