Attitude determination using Newton's method on Riemannian manifold

Attitude determination problem is formulated by Wahba as an optimization problem. This problem is reduced by Davenport to an eigenvalue and eigenvector problem of the K-matrix. Several popular solutions aiming at reducing the computational cost, such as QUEST and FOMA, use iterative algorithms of Newton's method to find the eigenvalue as the largest real root of the characteristic polynomial of K-matrix and then use some analytic formulas to calculate the eigenvector which is a function of the largest eigenvalue. Since the characteristic polynomial of K-matrix is a quartic, closed-form formulas for the eigenvalue were also available. However, extensive numerical experience shows that (a) the calculation of the eigenvector (quaternion) based on the analytic formulas is sensitive to the accuracy of the eigenvalue, and (b) directly solving the eigenvalue/eigenvector problem of the K-matrix is a much more robust method. To avoid the calculation of all eigenvectors, Newton algorithm on Riemannian manifold is proposed to calculate the largest eigenvalue and the corresponding eigenvector. Extensive simulation test is performed to demonstrate the effectiveness and efficiency of the algorithm.