numerically stable pseudoinverse algorithm in casadi

pseudoinverse_algorithm.py

import casadi as ca

def qr_eigen(A, iterations=100):
    pQ = ca.SX.eye(A.size1())
    X  = ca.SX(A)  # Make a copy in SX
    for _ in range(iterations):
        Q, R = ca.qr(X)   # QR decomposition in CasADi
        pQ   = ca.mtimes(pQ, Q)
        X    = ca.mtimes(R, Q)
    return ca.diag(X), pQ  # (eigenvalues, eigenvectors)

def pseudo_inverse_from_qr_eigen(M, tol=1e-12):
    """
    Compute a (Moore-Penrose) pseudoinverse of M
    assuming M is (approximately) diagonalizable via a real eigen-decomposition.
    """
    # Get eigenvalues (w) and eigenvectors (V)
    w_sym, V_sym = qr_eigen(M, iterations=200)  # more iterations for better convergence
    
    # Build reciprocal of eigenvalues with a tolerance to avoid division by zero
    inv_lambda = []
    for i in range(w_sym.numel()):
        # Symbolic "if" in CasADi can be done with if_else
        # or you can do something approximate like 1/(w + eps).
        inv_lambda_i = ca.if_else(w_sym[i] > tol, 1.0 / w_sym[i], 0.0)
        inv_lambda.append(inv_lambda_i)
    
    # Create diagonal matrix from inverse eigenvalues
    Lambda_inv = ca.diag(ca.vertcat(*inv_lambda))
    
    # Form the pseudo-inverse
    # M^+ = V * (Lambda^+) * V^T
    # Note: v_sym are columns = eigenvectors, so we use V^T on the right
    M_pinv = ca.mtimes(V_sym, ca.mtimes(Lambda_inv, V_sym.T))
    
    return M_pinv