Acta Mechanica Sinica

, Volume 34, Issue 2, pp 400–408 | Cite as

Numerical algorithm for rigid body position estimation using the quaternion approach

  • Miodrag Zigic
  • Nenad Grahovac
Research Paper


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.


Rigid body Kinematics Inertial measurement unit Euler parameters Quaternion 



The project was supported by the Serbian Ministry of Education, Science and Technological Development (Grant 174016).


  1. 1.
    Grauer, J.A., Hubbard Jr., J.E.: Flight Dynamics and System Identification for Modern Feedback Control. Woodhead Publishing, Philadelphia, USA (2013)CrossRefGoogle Scholar
  2. 2.
    Barshan, B., Durrant-Whyte, H.F.: Inertial navigation systems for mobiel robots. IEEE Trans. Robot. Autom. 11, 328–342 (1995)CrossRefGoogle Scholar
  3. 3.
    Beard, R.W.: State Estimation for Micro Air Vehicles, Studies in Computational Intelligence, vol. 70, 173–199. Springer, Berlin (2007)Google Scholar
  4. 4.
    Marins, J.L., Yun, X., Bachmann, E.R., et al.: An extended Kalman filter for quaternion-based orientation estimation using MARG sensors. In: Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems, Maui, Hawaii, USA, October 29–November 03 (2001)Google Scholar
  5. 5.
    Chao, H., Coopmans, C., Di, L., et al.: A comparative evaluation of low-cost IMUs for unmanned autonomus systems. In: 2010 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems, Salt Lake City, UT, USA, September 5–7 (2010)Google Scholar
  6. 6.
    Rehbinder, H., Hu, X.: Nonlinear state estimation for rigid-body motion with low-pass sensors. Syst. Control Lett. 40, 183–190 (2000)MathSciNetCrossRefzbMATHGoogle Scholar
  7. 7.
    Casson, A.J., Galvez, A.V., Jarchi, D.: Gyroscope versus accelerometer measurements of motion from wrist PPG during physical exercise. ICT Express 2, 175–179 (2016)CrossRefGoogle Scholar
  8. 8.
    Wang, X., Xiao, L.: Gyroscope-reduced inertial navigation system for flight vehicle motion estimation. Adv. Space Res. 59, 413–424 (2017)CrossRefGoogle Scholar
  9. 9.
    Izadi, M., Sanyal, A.K.: Rigid body pose estimation based on the Lagrange–d’Alembert principle. Automatica 71, 78–88 (2016)MathSciNetCrossRefzbMATHGoogle Scholar
  10. 10.
    Luinge, H.J., Veltink, P.H.: Inclination measurement of human movement using a 3-D accelerometer with autocalibration. IEEE Trans. Neural Syst. Rehabil. Eng. 12, 112–121 (2004)CrossRefGoogle Scholar
  11. 11.
    Ma, J., Kharboutly, H., Benali, A., et al.: Joint angle estimation with accelerometers for dynamic postural analysis. J. Biomech. 48, 3616–3624 (2015)CrossRefGoogle Scholar
  12. 12.
    Madgwick, S.O.H., Harrison, A.J.L., Vaidyanathan, R.: Estimation of IMU and MARG orientation using a gradient descent algorithm. In: 2011 IEEE International Conference on Rehabilitation Robotics, Rehab week Zurich, ETH Zurich Science City, Switzerland, June 29–July 1 (2011)Google Scholar
  13. 13.
    Sabatini, A.M.: Estimating three-dimensional orientation of human body parts by inertial/magnetic sensing. Sensors 11, 1489–1525 (2011)CrossRefGoogle Scholar
  14. 14.
    Santhiranayagam, B.K., Lai, D.T.H., Sparrow, W.A., et al.: A machine learning approach to estimate minimum toe clearance using inertial measurement units. J. Biomech. 48, 4309–4316 (2015)CrossRefGoogle Scholar
  15. 15.
    Esser, P., Dawes, H., Collett, J., et al.: IMU: inertial sensing of vertical CoM movement. J. Biomech. 42, 1578–1581 (2009)CrossRefGoogle Scholar
  16. 16.
    Hamilton, W.R.: On quaternions. Proc. R. Irish Acad. 3, 1–16 (1847)Google Scholar
  17. 17.
    Goldman, R.: Rethinking Quaternions: Theory and Computation, Synthesis Lectures on Computer Graphics and Animation. Morgan & Claypool Publishers, San Rafael (2010)Google Scholar
  18. 18.
    Kuipers, J.B.: Quaternions and Rotation Sequences. Princeton University Press, Princeton (1999)zbMATHGoogle Scholar
  19. 19.
    Mukundan, R.: Quaternions: from clasical mechanics to computer graphics, and beyond. In: Proceedings of the 7th Asian Technology Conference in Mathematics, Melaka, Malaysia, December 17–21 (2002)Google Scholar
  20. 20.
    Spring, K.W.: Euler parameters and the use of quaternon algebra in the manipulation of finite rotations: a review. Mech. Mach. Theory 21, 365–373 (1986)CrossRefGoogle Scholar
  21. 21.
    Fourati, H., Manamanni, N., Afilal, L., et al.: Rigid body motions estimation using inertial sensors: bio-logging application. In: Proceedings of the 7th IFAC Symposium on Medelling and Control in Biomedical Systems, Aalborg, Denmark, August 12–14 (2009)Google Scholar
  22. 22.
    Fourati, H., Manamanni, N., Afilal, L., et al.: Sensing technique of dynamic marine mammal’s attitude by use of low-cost inertial and magnetic sensors. In: 8th IFAC Conference on Control Applications in Marine Systems, Rostock-Warnemünde, Germany, September 15–17 (2010)Google Scholar
  23. 23.
    Chou, J.C.K.: Quaternion kinematic and dynamic differential equations. IEEE Trans. Robot. Autom. 8, 53–64 (1992)CrossRefGoogle Scholar
  24. 24.
    Betsch, P., Siebert, R.: Rigid body dynamics in terms of quaternions: Hamiltonian formulation and conserving numerical integration. Int. J. Numer. Methods Eng. 79, 444–473 (2009)MathSciNetCrossRefzbMATHGoogle Scholar
  25. 25.
    Nielsen, M.B., Krenk, S.: Conservative integration of rigid body motion by quaternion parameters with implicit constraints. Int. J. Numer. Methods Eng. 92, 734–752 (2012)MathSciNetCrossRefzbMATHGoogle Scholar
  26. 26.
    Xu, X., Zhong, W.: On the numerical influences of inertia representation for rigid body dynamics in terms of unit quaternion. J. Appl. Mech. 83, 061006-1–061006-11 (2016)Google Scholar
  27. 27.
    Sherif, K., Nachbagauer, K., Stainer, W.: On the rotational equations of motion in rigid body dynamics when using Euler parameters. Nonlinear Dyn. 81, 343–352 (2015)MathSciNetCrossRefzbMATHGoogle Scholar
  28. 28.
    Diebel, J.: Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors. Stanford University, Stanford (2006)Google Scholar
  29. 29.
    Durham, W.: Aircraft Flight Dynamics and Control. Wiley, New York (2013)Google Scholar
  30. 30.
    Goldstein, H.: Classical Mechanics. Addison-Wesley Publishing Company, Boston (1980)zbMATHGoogle Scholar
  31. 31.
    Cooke, J.M., Zyda, M.J., Pratt, D.R., et al.: NPSNET: flight simulation dynamics modeling using quaternions. Presence 1, 404–420 (1994)CrossRefGoogle Scholar
  32. 32.
    Gantmacher, F.: Lectures in Analytical Mechanics. Mir Publishers, Moscow (1975)Google Scholar

Copyright information

© The Chinese Society of Theoretical and Applied Mechanics; Institute of Mechanics, Chinese Academy of Sciences and Springer-Verlag GmbH Germany 2017

Authors and Affiliations

  1. 1.Department of Mechanics, Faculty of Technical SciencesUniversity of Novi SadNovi SadSerbia

Personalised recommendations