Numerical algorithm for rigid body position estimation using the quaternion approach

This paper deals with rigid body attitude estimation on the basis of the data obtained from an inertial measurement unit mounted on the body. The aim of this work is to present the numerical algorithm, which can be easily applied to the wide class of problems concerning rigid body positioning, arising in aerospace and marine engineering, or in increasingly popular robotic systems and unmanned aerial vehicles. Following the considerations of kinematics of rigid bodies, the relations between accelerations of different points of the body are given. A rotation matrix is formed using the quaternion approach to avoid singularities. We present numerical procedures for determination of the absolute accelerations of the center of mass and of an arbitrary point of the body expressed in the inertial reference frame, as well as its attitude. An application of the algorithm to the example of a heavy symmetrical gyroscope is presented, where input data for the numerical procedure are obtained from the solution of differential equations of motion, instead of using sensor measurements.