Fast AHRS Filter for Accelerometer, Magnetometer, and Gyroscope Combination with Separated Sensor Corrections

Josef Justa, Václav Smídl, Ales Hamácek2020 · DOI

TL;DR

This paper introduces a novel, computationally efficient Attitude and Heading Reference System (AHRS) filter that improves accuracy by independently processing accelerometer and magnetometer data before fusing them. This approach leads to better performance, especially in scenarios with slow rotations and dynamic accelerations, outperforming existing state-of-the-art methods.

What Problem Does This Paper Address?

Accurate estimation of an object's orientation (attitude and heading) is crucial for many applications, from robotics and virtual reality to aerospace. Attitude and Heading Reference Systems (AHRS) typically achieve this by fusing data from gyroscopes, accelerometers, and magnetometers. However, each sensor has limitations: gyroscopes drift over time, accelerometers are affected by dynamic movements, and magnetometers are susceptible to magnetic disturbances. Existing fusion methods often struggle to effectively reject these disturbances, leading to reduced accuracy, especially in challenging real-world scenarios. This paper aims to address this by developing a more robust and accurate AHRS filter that can better handle sensor noise and disturbances.

Methods

The core innovation of this paper lies in a new predictor-corrector filter structure. Unlike traditional methods that might process sensor corrections jointly, this filter separates the correction steps for the accelerometer and magnetometer.

Here's a breakdown of the key methodological aspects:

  • Predictor-Corrector Structure: The filter uses gyroscope data for prediction (estimating the next orientation state) and accelerometer/magnetometer data for correction (refining the predicted state).
  • Separated Sensor Corrections: This is the main novelty. Instead of fusing accelerometer and magnetometer corrections together, they are processed independently. This allows for more tailored handling of each sensor's specific disturbances.
  • Quaternion-Based Operations: The filter heavily relies on quaternion mathematics for representing rotations, which is a standard and efficient approach in AHRS.
  • Two Filter Variants:
    • SCF (Separated Correction Filter): Uses accurate mathematical operations for highest possible accuracy.
  • FSCF (Fast Separated Correction Filter): Employs linearized approximations of some operations to achieve faster computation, making it suitable for embedded systems.
  • Gradient Descent Approach: The correction mechanism is based on a gradient descent principle, similar to the Madgwick filter, but with modifications to handle the separated sensor inputs.
  • Novel Fusion of Corrections: A new analytical formula is proposed to fuse the independent correction quaternions, aiming for better accuracy than simple quaternion multiplication.

Datasets

The authors created a new benchmark dataset for evaluating their filter.

  • Experimental Setup: Data was collected using a custom experimental platform equipped with a low-cost MARG sensor (BMX055) and an optical positioning system (VICON-460) for ground truth.
  • Scenarios: Three distinct experimental scenarios were designed to capture different motion characteristics:
  • Slow Rotations: Mimics applications like indoor drone navigation.
  • Slow and Fast Rotations: Represents more dynamic environments like virtual reality.
  • Slow/Fast Rotations with Dynamic Accelerations: The most challenging scenario, simulating harsh conditions found in inertial motion capture.
  • Availability: The recorded datasets and the source code for the SCF and FSCF filters are made publicly available.

Key Insights and Ideas

  • Independent Correction is Key: The most significant insight is that processing accelerometer and magnetometer corrections separately, rather than jointly, leads to substantial accuracy improvements. This suggests that each sensor's unique disturbance characteristics can be better managed when addressed individually.
  • Tuning Matters, But Less So for the Proposed Filter: While filter parameter tuning is crucial for most AHRS algorithms, the proposed filter (especially with the constant correction function) showed less sensitivity to tuning and performed well in cross-validation studies. This makes it more practical for real-world deployment.
  • Correction Function Choice is Critical: The paper highlights that the choice of the correction function (linear, constant, or segmented) significantly impacts filter performance. The constant correction function was found to be a good balance between performance and simplicity for standard applications.
  • Weighting Matters: The optimal weighting of the magnetometer correction was found to be significantly smaller (around 10 times) than the accelerometer correction. This implies that filters using a common weight for both are sub-optimal.

Key Results

The proposed SCF and FSCF filters demonstrated superior performance compared to several state-of-the-art methods, including Madgwick's complementary filter, Valenti's complementary filter, and Guo's Fast Kalman Filter.

  • Accuracy:
    • In the scenario with slow rotations, the proposed filter achieved a 77% lower mean absolute error (MAE) than the second-best filter.
  • Across all scenarios, the proposed filter consistently showed the lowest MAE and RMSE. For instance, in Scenario 3 (slow/fast rotations and dynamic acceleration), the proposed filter's MAE was 3.37 deg, while the next best (Guo) was 3.94 deg.
  • Computational Cost:
    • The FSCF variant is particularly efficient, requiring only 160 floating-point operations per filter step, making it one of the fastest filters tested.
  • The SCF variant is also competitive with 179 FLOPs, offering a good trade-off between accuracy and speed.
  • Robustness: The filter showed particular strength in handling dynamic accelerations and slow rotations.

Limitations and Future Work

  • Computational Cost of Accurate SO(3) Operations: While the SCF variant is accurate, its computational cost might still be a concern for extremely resource-constrained embedded systems compared to the FSCF.
  • On-line Adaptation of Weights: The authors suggest that future research could explore on-line adaptation of the correction weights, which could further enhance performance in highly dynamic or unpredictable environments.
  • Impact of Sampling Frequency: The paper notes that the benefits of accurate SO(3) operations (like the proposed fusion formula) might become more pronounced at lower sampling frequencies. Further investigation into this aspect could be valuable.

Summary updated

BibTeX
@misc{justa2020fast,
  author = {Josef Justa and Václav Smídl and Ales Hamácek},
  title = {{Fast AHRS Filter for Accelerometer, Magnetometer, and Gyroscope Combination with Separated Sensor Corrections}},
  year = {2020},
  doi = {10.3390/s20143824},
}
Figure