A Matrix-Based Sensor Fusion Algorithm for Nine-D.o.F. IMU Attitude Estimation

The orientation of a body in space can be conveniently determined through the fusion of different and redundant sensors; for example, as in Inertial Measurement Units (IMUs) and Magnetic, Angular Rate, and Gravity (MARG). Although this problem has been already addressed by many authors, and many relevant algorithms are available nowadays, this paper explores the possibility of improving their computational efficiency and reliability by suggesting some alternative refinements: (a) A matrix-based reformulation that speeds up the code by avoiding quaternion library operations, exploiting the well-established correspondence between quaternion algebra and linear matrix operators; (b) the use of a necessary condition between Euler parameters and their time derivatives as a kinematic-consistency residual; (c) the minimization of this norm; and (d) the possibility of decoupling the accelerometer sensor data in case the acceleration is higher than the gravity intensity. The matrix-based reformulation significantly improves the execution time of the original algorithms, while the optional refinement steps trade most of this saving for a consistency norm with machine precision. The algorithms are compared on a publicly available benchmark dataset and validated experimentally against a goniometric mechanical reference.