A novel constrained quaternion filter

This paper presents a novel quaternion filter from vector measurements that belongs to the realm of deterministic constrained least-squares estimation. Hinging on the interpretation of quaternion measurements errors as angular errors in a four-dimensional Euclidean space, a novel cost function is developed and a minimization problem is formulated under the quaternion unit-norm constraint. This approach sheds a new light on the Wahba problem and on the q-method. The optimal estimate can be interpreted as achieving the least angular distance among a collection of planes in ℝ⁴ that are constructed from the vector observations. The resulting batch algorithm is mathematically equivalent to the q-method. Yet, taking advantage of the gained geometric insight, a recursive algorithm is developed, where the update stage consists of a rotation in the four-dimensional Euclidean space. The rotation angle is empirically designed as a fading memory factor. The quaternion update stage is multiplicative thus preserving the estimated quaternion unit-norm and no iterative search for eigenvalues is required as opposed to the q-method. Simulations illustrate the convergence and accuracy properties of the proposed algorithm.