RosenbergerAlgorithm.py 文件源码

def _update_(U, D, d, lambda_):
    """Go from u(n) to u(n+1)."""
    I = np.identity(3)

    m = U.T.dot(d)
    p = (I - U.dot(U.T)).dot(d)
    p_norm = np.linalg.norm(p)

    # Make p and m column vectors
    p = p[np.newaxis].T
    m = m[np.newaxis].T

    U_left = np.hstack((U, p/p_norm))
    Q = np.hstack((lambda_ * D, m))
    Q = np.vstack((Q, [0, 0, p_norm]))

    # SVD
    U_right, D_new, V_left = np.linalg.svd(Q)

    # Get rid of the smallest eigenvalue
    D_new = D_new[0:2]
    D_new = np.diagflat(D_new)

    U_right = U_right[:, 0:2]

    return U_left.dot(U_right), D_new