Adaptive-gain complementary filter of inertial and magnetic data for orientation estimation

Accurate estimation of orientation based on data from small low-cost strapdown inertial and magnetic sensors is often inaccurate during highly dynamic motion or when trying to track movements that include two or more periods characterized by significantly different frequencies. This paper presents a complementary filtering algorithm for estimating orientation based on inertial/magnetic sensor measurements. The algorithm takes advantage of the complementary nature of the information offered by high-frequency angular rate sensor data and low frequency accelerometers and magnetometers. The filtering algorithm utilizes a single gain that can be adaptively adjusted to achieve satisfactory performance while tracking two or more different types of motion. An additional feature of our approach involves the simple estimation of the gyro bias during periods exhibiting low dynamics and its subsequent use to correct the instantaneous gyro measurements. Simulation and experimental results are presented that demonstrate the performance of the algorithm during slow or nearly static movements, as well as, those which are highly dynamic. Experimental results indicate that the algorithm is able to track pitch and roll during dynamic motion with an RMS error of less than two degrees. This is believed to be superior to current proprietary commercial algorithms.