Quaternion estimator algorithm
The quaternion estimator algorithm (QUEST) is an algorithm designed to solve Wahba's problem, that consists of finding a rotation matrix between two coordinate systems from two sets of observations sampled in each system respectively. The key idea behind the algorithm is to find an expression of the loss function for the Wahba's problem as a quadratic form, using the Cayley–Hamilton theorem and the Newton–Raphson method to efficiently solve the eigenvalue problem and construct a numerically stable representation of the solution. The algorithm was introduced by Malcolm D. Shuster in 1981, while working at  Computer Sciences Corporation. While being in principle less robust than other methods such as Davenport's q method or singular value decomposition, the algorithm is significantly faster and reliable in practical applications, and it is used for attitude determination problem in fields such as robotics and avionics. Formulation of the problem. Wahba's problem consists of finding a rotation matrix formula_1 that minimises the loss function where formula_3 are the vector observations in the reference frame, formula_4 are the vector observations in the body frame, formula_5 is a rotation matrix between the two frames, and formula_6 are a set of  weights such that formula_7. It is possible to rewrite this as a maximisation problem of a gain function formula_8 defined in such a way that the loss formula_10 attains a minimum when formula_8 is maximised. The gain formula_8 can in turn be rewritten as where formula_14 is known as the attitude profile matrix. In order to reduce the number of variables, the problem can be reformulated by parametrising the rotation as a unit quaternion formula_15 with vector part formula_16 and scalar part formula_17, representing the rotation of angle formula_18 around an axis whose direction is described by the vector formula_19 , subject to the unity constraint formula_20. It is now possible to express formula_5 in terms of the quaternion parametrisation as where formula_23 is the skew-symmetric matrix Substituting formula_5 with the quaternion representation and simplifying the resulting expression, the gain function can be written as a quadratic form in formula_26 where the formula_28 matrix is defined from the quantities This quadratic form can be optimised under the unity constraint by adding a Lagrange multiplier formula_31, obtaining an unconstrained gain function that attains a maximum when This implies that the optimal rotation is parametrised by the quaternion formula_34 that is the  eigenvector associated to the largest eigenvalue formula_35 of formula_36. Solution of the characteristic equation. The optimal quaternion can be determined by solving the characteristic equation of formula_36 and constructing the eigenvector for the largest eigenvalue. From the definition of formula_36, it is possible to rewrite as a system of two equations where formula_41 is the Rodrigues vector. Substituting formula_42 in the second equation with the first, it is possible to derive an expression of the characteristic equation Since formula_44, it follows that formula_45 and therefore formula_46 for an optimal solution (when the loss formula_10 is small). This permits to construct  the optimal quaternion formula_34 by replacing formula_35 in the Rodrigues vector formula_42 The formula_42 vector is however singular for formula_53. An alternative expression of the solution that does not involve the Rodrigues vector can be constructed using the Cayley–Hamilton theorem. The characteristic equation of a formula_54 matrix formula_55 is where The Cayley–Hamilton theorem states that any square matrix over a commutative ring satisfies its own characteristic equation, therefore allowing to write where and for formula_61 this provides a new construction of the optimal vector that gives the quaternion representation of the optimal rotation as where The value of  formula_35 can be determined as a numerical solution of the characteristic equation. Replacing formula_66 inside the previously obtained characteristic equation gives where whose root can be efficiently approximated with the Newton–Raphson method, taking 1 as initial guess of the solution in order to converge to the highest eigenvalue (using the fact, shown above, that formula_46 when the quaternion is close to the optimal solution).