A simplified quaternion-based algorithm for orientation estimation from earth gravity and magnetic field measurements

Orientation of a static or slow-moving rigid body can be determined from the measured gravity and local magnetic field vectors. Some formulation of the QUaternion ESTimator (QUEST) algorithm is commonly used to solve this problem. Triads of accelerometers and magnetometers are used to measure gravity and local magnetic field vectors in sensor coordinates. In the QUEST algorithm, local magnetic field measurements affect not only the estimation of yaw but also that of roll and pitch. Due to the deviations in the direction of the magnetic field vector between locations, it is not desirable to use magnetic data in calculations that are related to the determination of roll and pitch. This paper presents a geometrically intuitive 3-degree-of-freedom (3-DOF) orientation estimation algorithm with physical meaning [which is called the factored quaternion algorithm (FQA)], which restricts the use of magnetic data to the determination of the rotation about the vertical axis. The algorithm produces a quaternion output to represent the orientation. Through a derivation based on half-angle formulas and due to the use of quaternions, the computational cost of evaluating trigonometric functions is avoided. Experimental results demonstrate that the proposed algorithm has an overall accuracy that is essentially identical to that of the QUEST algorithm and is computationally more efficient. Additionally, magnetic variations cause only azimuth errors in FQA attitude estimation. A singularity avoidance method is introduced, which allows the algorithm to track through all orientations.