A Robust Quaternion based Kalman Filter Using a Gradient Descent Algorithm for Orientation Measurement

摘要

This paper presents a robust quaternion based filter approach to estimate orientation from Magnetic Angular Rate Gravity (MARG)-sensors. These sensors consist of tri-axis accelerometer and gyroscope as well as a tri-axis magnetometer which allow a complete measurement of orientation relative to the direction of gravity and magnetic field of the earth. The proposed filter uses a gradient descent algorithm (GDA) to compute an orientation from magnetometer and accelerometer data. This calculated orientation is directly used as an input in a Kalman filter framework (KFF) which predicts the orientation estimation from gyroscope data. The embedding of the gradient descent algorithm into the Kalman filter allows the computation of a weighted orientation represented as quaternion. Furthermore, the designed filter can overcome short time magnetic disturbance by switching between MARG and IMU equations inside the gradient descent filter stage (GDFS) and therefore enables a more robust orientation estimation without the need for additional algorithms. Tests show the proposed filter to be superior to a commercially available sensor fusion algorithm related to orientation estimation at slow angular rates. Moreover, the proposed filter is able to maintain good orientation estimation under short term magnetic disturbance.