Open AccessArticle

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

Josef Justa

^1,*

Václav Šmídl

and

Aleš Hamáček

Department of Measurement and Technology, University of West Bohemia, 30100 Plzen, Czech Republic

RICE, University of West Bohemia, 30100 Plzen, Czech Republic

Author to whom correspondence should be addressed.

Sensors 2020, 20(14), 3824; https://doi.org/10.3390/s20143824

Submission received: 20 April 2020 / Revised: 24 June 2020 / Accepted: 7 July 2020 / Published: 9 July 2020

(This article belongs to the Section Physical Sensors)

Download

Browse Figures

Review Reports Versions Notes

Abstract

A new predictor–corrector filter for attitude and heading reference systems (AHRS) using data from an orthogonal sensor combination of three accelerometers, three magnetometers and three gyroscopes is proposed. The filter uses the predictor—corrector structure, with prediction based on gyroscopes and independent correction steps for acceleration and magnetic field sensors. We propose two variants of the filter: (i) one using mathematical operations of special orthogonal group SO(3), that are accurate for nonlinear operations, for highest possible accuracy, and (ii) one using linearization of nonlinear operations for fast evaluation. Both approaches are quaternion-based filter realizations without redundant steps. The filters are compared to state of the art methods in this field on data recorded using low-cost microelectromechanical systems (MEMS) sensors with ground truth measured by the VICON optical system. Both filters achieved better accuracy than conventional methods at lower computational cost. The recorded data with ground truth reference and the source codes of both filters are publicly available.

Keywords:

attitude estimation; complementary filter; gradient descent filter; sensor fusion

1. Introduction

Digital attitude and heading reference systems (AHRS) are an essential component of navigation and localization systems. It is typically based on the measurements of inertial motion units consisting of gyroscopes and accelerometers in combination with magnetometers. This combination is known as the magnetic angular rate and gravity sensor combination (MARG) or magnetic inertial motion unit (MIMU). The MARG combination takes advantage of a diverse direction of gravity acceleration vector and magnetic vector field in the majority of locations on Earth (the exception being magnetic poles). The AHRS system aims to map orientation between the object of interest (body coordinate) and the coordinate system defined by the gravitational vector and the local dominant magnetic field vector.

Initial approaches to this problem were based on the analysis of instantaneous measurement. A landmark paper by Grace Wahba defined the main rotational problem for spacecraft attitude [1] in 1965. The aim is to find the right rotation matrix between two sets of vectors with the minimum weighted square error. Many follow-up contributions focused on the efficient solution of this problem, including the QUaternion ESTimator (QUEST) [2], the ESOQ algorithm, which solves the polynomial characteristic equation of QUEST in one step [3], and many others [4]. Since the first solution, the AHRS could start to grow into various fields and combine multiple sources of information. Mims and Farrell used the IMU reference for synthetic aperture radars in the 1970s [5]. In 2000, the magnetometer was used to obtain heading in a system without the gyroscope [6]. The combination of all three types (MARG) was used in 2004 by Gebre-Egziabher [7].

However, since measurements of all sensors are susceptible to disturbances, the challenge of any fusion method is to reject these disturbances as much as possible. This is often achieved using by a combination of different temporal characteristics of individual sensors. While a gyroscope provides excellent information about the rapid changes of the orientation, it provides only relative changes of orientation that are subject to slow drift varying with its lifetime and with temperature. On the other hand, an accelerometer and magnetometer provide a direct measurement of orientation; however, the measurement cannot be used as a reference at every instant since the accelerometer measures the gravity acceleration vector contaminated by dynamic acceleration and the magnetometer measures the Earth magnetic field contaminated by local magnetic anomalies and various magnetic disturbances. This contamination is also considered a disturbance and needs to be rejected. The character of disturbances may differ in different application scenarios. For example, dynamic accelerations of a MARG mounted on the foot of a running man would be different to that of a MARG mounted on an aircraft or UAV. Each sensor type is also influenced by orthogonality misalignment, cross-axis sensitivity deviation, and amplitude and phase attenuation over frequency. Our primary application is the human motion recognition domain [8,9] so we focus our attention on the specifics of this domain such as dynamic acceleration disturbance.

The majority of the fusion methods follow the predictor–corrector structure, where the gyroscope measurements are used in the prediction step and the accelerometer and magnetometer measurements in the correction step. Various classes of design methods for these filters are used, such as the Kalman filter approach, complementary filter, or gradient-based predictor–corrector filters. These approaches differ in their assumptions made on the character of the disturbances.

The Kalman filter is based on stochastic filtering theory, which assumes that the disturbance follows the Gaussian distribution of both the state evolution error and observation errors. It is a classical approach [10] used in the human motion domain with a large number of variants, see e.g., [11] for their comparison. Various extensions focus on different aspects of the filtering differing such as the effect of nonlinear models that is addressed by the more computationally costly unscented Kalman Filter [12]. However, the limiting factor of Kalman-based filters is the underlying assumption of Gaussian distributed disturbance. This has been addressed by extensions proposing adaptive estimates of the covariance matrices, see e.g., [13] for an excellent review. The approaches are based on various approaches monitoring the evolution of estimation errors using fuzzy rules [13], moving average covariance estimation [12], Markov chains [14], or segmented moving average covariance estimators [15] to name a few examples. The performance benefit of these methods usually comes with a significant computational price, making it unsuitable for low cost, low-power applications. While multiple techniques decreasing computational cost of the Kalman filters exists [16], adaptation of covariance matrices is always expensive.

The complementary filtering [17,18,19] is using low- and/or high-pass filters to remove disturbances from the measurements. The disturbance is thus modeled as any signal that does not pass through the filter. These filters are computationally much cheaper than Kalman filters, see e.g., [20]. The tuning of those filters is still complicated, especially in time-varying scenarios where the character of disturbances changes.

The third principal direction of handling the disturbance is the gradient-based approach pioneered by Madgwick [21], which is based on the assumption that valid measurement is only within a predefined interval and anything beyond is a disturbance. While other gradient-based approaches exist, using different update rules such as Gauss–Newton [22], the approach of [21] is unique in the recommendation to normalize the gradient, which is a computationally cheap operation. Similar ideas are known in machine learning as gradient clipping [23]. Its excellent empirical performance was recently explained using the assumption of relaxed Lipschitz constant of the optimized cost function [24].

In this paper, we follow the approach presented in [21], since it corresponds to our experience that both accelerometer and magnetometer suffer from severe disturbances. Since these disturbances do not follow any predefined probability distribution, neither frequency spectrum, the limitation of their amplitude (i.e., limit on the Lipschitz constant of the dynamical model) seems a reasonable assumption to make. However, in [21] this assumption is applied to the difference between gyroscope prediction and fused measurement vector that is obtained using the solution of the Wahba problem. We conjecture that this is suboptimal since the amplitude of the disturbance affects the direction of the correction step. Therefore, we designed a new correction step that puts a hard limit on the amplitude of the disturbance of individual sensors. Similar ideas on the separation of the sensor influence have been proposed for the Kalman filtering approach in [25,26], with a different assumption on the noise. It is also related to various two-step approaches [27], however, in a more principled manner.

The contribution of our paper is as follows:

We propose a new gradient-based filter for AHRS with the following features: (i) the gradient of correction from magnetometer and accelerometer are processed independently, (ii) the step size of the gradient descent is limited by the correction function independently for each sensor, and (iii) the correction vectors are fused using a new approximation of the correct SO(3) operation. Separation of the correction steps from each sensor implied heavy modification of the original filter, as visualized in Figure 1.
For each operation that is used in the design of the proposed filter, we analyzed the accuracy of its implementation in the SO(3) group. We provide two approximations: (i) an accurate approximation of the correct quaternion operation as possible, and (ii) its approximation simplified using e.g., linearization. The proposed filter is presented in two versions: a separated correction filter (SCF) using the accurate SO(3) operations, and a fast SCF (FSCF) that is composed of the simplified (linearized) operations.
We recorded measurements of the MARG sensor mounted on an experimental platform in a laboratory equipped with the optical tracking system VICON-460. We provide this data as an open-source benchmark. We compare the performance of the proposed filter with selected alternative filters and demonstrate that it has higher accuracy at a lower computational cost.

The remainder of this paper is organized as follows: Elementary operations with quaternions and their notation is reviewed in Section 2. The proposed filter is described in detail in Section 3 including a description of several possible approximations of the correct SO(3) operations. Experimental results are provided in Section 4, including details of the data measurement and comparison of selected competing filters. A conclusion from the experiments is drawn in Section 5.

2. Important Rotation Operations

In this section, the necessary rotational operations that are used in the designed filters are reviewed. For a full treatment of the subject, see [28]. First, rotation of normalized vectors in Euclidean space is presented, then elementary operations with quaternions are described. Unit vector a is a vector that has unit norm

| | a | | = a^{⊤} a = 1

, where

^{⊤}

denotes transposition. The direct angle (DA),

α_{a, b}

, between two unit vectors,

a = {[a_{x}, a_{y}, a_{z}]}^{⊤},

and

b = {[b_{x}, b_{y}, b_{z}]}^{⊤}

, is defined by

\begin{matrix} α_{a, b} & = {cos}^{- 1} (a^{⊤} b) . \end{matrix}

(1)

The cross product of two unit vectors a and b, is defined by

cross (a, b) = [a_{y} b_{z} - a_{z} b_{y}, a_{z} b_{x} - a_{x} b_{z}, a_{x} b_{y} - a_{y} b_{x}] .

(2)

The proposed filter is based on Hamilton’s quaternions. For implementation in this mathematical group, we use a unit quaternion which is a complex number representing rotation from the source coordinate frame f to the destination frame

f^{'}

which can be written in vector form

q_{f, f^{'}} = [q_{w}, q_{x}, q_{y}, q_{z}] = [cos \frac{θ}{2}, u_{x} i sin \frac{θ}{2}, u_{y} j sin \frac{θ}{2}, u_{z} k sin \frac{θ}{2}],

(3)

where unit vector u in coordinate frame f defines rotational axis (axis around rotation has been done) and

θ

the angle of rotation. The angle of rotation

θ

corresponds to the DA for 3D vectors.

The quaternion rotation q can be converted to the rotation matrix R using

R = [\begin{matrix} q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2} & 2 q_{x} q_{y} - 2 q_{w} q_{z} & 2 q_{x} q_{z} + 2 q_{w} q_{y} \\ 2 q_{x} q_{y} + 2 q_{w} q_{z} & q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2} & 2 q_{y} q_{z} - 2 q_{w} q_{x} \\ 2 q_{x} q_{z} - 2 q_{w} q_{y} & 2 q_{y} q_{z} + 2 q_{w} q_{x} & q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2} \end{matrix}]

(4)

The most important operations with quaternions are the quaternion product × and quaternion conjugation

q^{- 1}

, since they define the rotation of coordinates of vector v in frame f to coordinates

v^{'}

in frame

f^{'}

\begin{matrix} v^{'} & = q_{f, f^{'}}^{- 1} \times v \times q_{f, f^{'}} . & q_{a, c}^{- 1} & = [q_{w}, - q_{x}, - q_{y}, - q_{z}] = q_{c, a} . \end{matrix}

(5)

Here, v is a four-dimensional vector with the first element equal to zero followed by conventional coordinates in 3D space. The quaternion multiplication ×,

q_{f, f^{″}} = q_{f, f^{'}} \times q_{f^{'}, f^{″}},

(6)

is the result of the product of complex numbers corresponding to vectors (3). Since

q_{w} = cos \frac{θ}{2}

and

\sqrt{q_{x}^{2} + q_{y}^{2} + q_{z}^{2}} = sin \frac{θ}{2}

(3), the angle of rotation

θ

can be computed from a quaternion using:

\begin{matrix} θ (q) & = 2 arctg 2 (\sqrt{q_{x}^{2} + q_{y}^{2} + q_{z}^{2}}, q_{w}) . \end{matrix}

(7)

3. Filter Implementation

The filter is designed as a modification of the Madgwick filter where we split the contribution of each sensor to the correction. This requires a replacement of the solution of the Wahba problem. We thus cannot fuse information in the derivative form as the original filter, and we have to use the absolute form. Specifically, the correction step is composed of two important parts for each sensor independently: (i) determination of distance and direction of deviation from the prediction, and (ii) determination of the correction step size based on the shape of a correction function. The correction rotation is determined using a combination of the directions of deviation of individual sensors weighted by their correction step sizes.

The proposed approach has an independent weighting of their contributions to correction. Note that the correction parts from the accelerometer and magnetometer are almost completely separated except the magnetic reference vector determination (more in Section 3.3). There were some attempts to do the separation of sensor contributions in previous works, such as [26], with which we compare our filter in the experimental part.

The proposed filter can be implemented using either quaternions or rotational matrices. We present the majority of operations in the quaternion formulation and use the rotational matrix in cases where it is computationally advantageous. Due to limited computing power in embedded systems (the most common application), we present two versions of the filter. The separated correction filter (SCF) using as accurate numerical operations, and the fast separated correction filter (FSCF) using computationally efficient approximations of these operations. The influence of those approximations was studied in simulations. The structure of those two variants is identical, the difference is only in details of the implementation of individual blocks (Figure 1 right). The output of the filter is quaternion

{\hat{q}}_{t}

defining rotation from the estimated coordinate frame of the sensor body to the earth coordinate frame. Here, t is used as the index of the time step. Implementation of each block from Figure 1 (right) is described as follows.

3.1. Dynamic Rotation Change (qDot)

Transformation of the gyroscopic rotation rate data to the small rotation difference is provided by the dynamic rotation block (qDot). The function can also be interpreted as a transformation of the gyroscopic measurement to the rotational quaternion. The first step is the transformation of the gyroscopic data to Euler angles (yaw, pitch, and roll, respectively) in the body coordinate frame:

[\begin{matrix} α \\ β \\ γ \end{matrix}] = ω_{t} d t,

(8)

where

d t

is the sampling rate. The second step is the transformation of the Euler angles to the quaternion by

q_{d o t, x} = sin (\frac{α}{2}), q_{d o t, y} = sin (\frac{β}{2}), q_{d o t, z} = sin (\frac{γ}{2}),

(9)

q_{d o t, w} = \sqrt{1 - (q_{d o t, x}^{2} + q_{d o t, y}^{2} + q_{d o t, z}^{2})},

(10)

where

q_{d o t}

is the rotation between coordinate frame of the body at time

t - 1

and time t.

The Equations (9) and (10) are used in SCF. Due to small angles (fast sampling rate) it is possible to linearize the sine functions in (9) for the use in FSCF

{\tilde{q}}_{d o t} = 1 + \frac{α}{2} i + \frac{β}{2} j + \frac{γ}{2} k .

(11)

The impact of this approximation is negligible in real conditions.

3.2. Prediction Integration (qPred)

Prediction of the new position reference frame of the sensor based on information from the gyroscope is obtained by rotation integration in SO(3) which is defined by quaternion multiplication (6),

\begin{matrix} q_{p r e d} & = {\hat{q}}_{t - 1} \times q_{d o t}, \end{matrix}

(12)

where

q_{p r e d}

is a quaternion of rotation from the predicted frame of the body to the earth coordinate frame.

3.3. Accelerometer Reference Vector (aRef, accPred)

We assume that the measured vectors of acceleration are normalized at the beginning of each filter step (Sensors have to be calibrated before measurement, especially the magnetometer). By the reference vector of the accelerometer, we define the part of the earth coordinate frame. The reference vector can be set as

a_{r} = {[0, 0, 1]}^{⊤},

(13)

which represents zero rotation if the gravitational acceleration is pointing down along the ‘z’ axis of the body reference frame.

The reference vector in the predicted body frame can be obtained by quaternion rotation via Equation (5)

a_{p r e d} = q_{p r e d}^{- 1} \times a_{r} \times q_{p r e d} .

(14)

However, for the specific choice of the reference vector (13), the same operation using rotation matrices has a lower computational cost. Therefore, we implement (14) in block “accPred” using

a_{p r e d} = R^{⊤} a_{r} = [\begin{matrix} 0 & 0 & 2 q_{x} q_{z} - 2 q_{w} q_{y} \\ 0 & 0 & 2 q_{y} q_{z} + 2 q_{w} q_{x} \\ 0 & 0 & 1 - 2 (q_{x}^{2} + q_{y}^{2}) \end{matrix}] [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] .

(15)

3.4. Magnetometer Reference Vector (mRef, magPred)

The choice of the reference vector for the magnetometer is more complicated. Due to magnetic distortions, influences by ferromagnetic materials in buildings and variation of magnetic inclination, it is not appropriate to define the magnetic reference vector by a constant.

We follow [21], and choose the reference of the magnetometer using the predicted reference frame. The original reference proposed in [21] comes with a significant computational cost. Therefore, we propose to compute the reference in the body frame instead of the Earth frame. To achieve this, we need to rotate the accelerometer to the body frame. We use the predicted value of the accelerometer for suppression of the measurement noise. The result is a computationally cheaper alternative since its components are already available:

\begin{matrix} m_{r, z} = & cos (\hat{φ}) = a_{p r e d, x} m_{t, x} + a_{p r e d, y} m_{t, y} + a_{p r e d, z} m_{t, z}, \\ m_{r, x} = & sin (\hat{φ}) = \sqrt{1 - m_{r, z}^{2}}, \\ m_{r} = & {[m_{r, x}, 0, m_{r, z}]}^{⊤}, \end{matrix}

(16)

where

\hat{φ}

is the estimate of the current inclination between the predicted accelerometer and magnetometer measurement using (1). This formulation is computationally faster and the experiments show that it has a negligible impact on the accuracy of the resulting filter.

Rotation of the reference vector to the predicted coordinate frame is obtained analogically to (15) as

m_{p r e d} = R^{⊤} m_{r} = [\begin{matrix} 1 - 2 (q_{y}^{2} + q_{z}^{2}) & 0 & 2 q_{x} q_{z} - 2 q_{w} q_{y} \\ 2 q_{x} q_{y} - 2 q_{w} q_{z} & 0 & 2 q_{y} q_{z} + 2 q_{w} q_{x} \\ 2 q_{x} q_{z} + 2 q_{w} q_{y} & 0 & 1 - 2 (q_{x}^{2} + q_{y}^{2}) \end{matrix}] [\begin{matrix} m_{r, x} \\ 0 \\ m_{r, z} \end{matrix}] .

(17)

3.5. Deviation of Prediction and Measurement (magDev, accDev)

If the predicted reference frame is correct, the predicted values of the magnetometer and accelerometer reference vectors should be close to the measured values. We quantify the deviation of the measured vectors from the reference vectors by angle and direction. For example, for the accelerometer, the deviation between the measured vector

a_{t}

and the predicted vector

a_{p r e d}

is computed as

\begin{matrix} α_{a} = & {cos}^{- 1} (a_{t}^{⊤} a_{p r e d}), & a_{c o r} = & \frac{cross (a_{t}, a_{p r e d})}{∥ cross (a_{t}, a_{p r e d}) ∥}, \end{matrix}

(18)

where cross is defined in (2). The same operations are performed for measurements of

m_{t}

and reference vectors

m_{r}

of the magnetometer, yielding angle

α_{m}

and

m_{c o r}

. These operations are implemented by blocks “accDev” and “magDev” in Figure 1.

3.6. Correction Step Determination (magCor, accCor)

Deviation of the measurement from the prediction is an indicator in which direction we should rotate the predicted coordinate frame to obtain a better estimate. The key component of the filter is the choice of the influence of the deviation on the correction of the predicted coordinate frame. In our filter, we propose a correction in the direction of vectors

a_{c o r}

and

m_{c o r}

but decrease the scale of the correction (angles

α_{a}

and

α_{m}

) by a correction function, e.g.,

β_{a} = f_{c o r} (α_{a}, λ_{a}),

(19)

for the accelerometer deviation angle. Here,

λ_{a}

denotes a tuning parameter. The choice of function

f_{c o r}

has a significant impact on the result. We will test three basic correction functions: (i) the linear function

β_{a} = λ_{a} α_{a}

(intercept zero); (ii) the constant function

β_{a} = λ_{a}

(Madgwick approach); and (iii) the segmented function as a combination two previous:

β_{a} = λ_{a, 1} α_{a},

λ_{a, 1} α_{a} < λ_{a, 2}

β_{a} = λ_{a, 2}

otherwise. The most commonly used method in other filters is the linear correction step. Parameters

λ_{a}

and

λ_{m}

can be considered as the filter correction weights of each sensor.

Correction rotation from the accelerometer measurement can be formulated in the form of a quaternion (3) as follows:

q_{a, c o r} = [cos \frac{β_{a}}{2}, a_{c o r}^{⊤} sin \frac{β_{a}}{2}] .

(20)

Since angle

β_{a}

is typically very small, we can approximate (20) by linearization

{\tilde{q}}_{a, c o r} = [1, a_{c o r}^{⊤} \frac{β_{a}}{2}],

(21)

which is used in the simplified FSCF. Analogical equations are used for the rotation towards the measurement of the magnetometer,

q_{m, c o r}

It is not obvious, but in his famous paper [21], Madgwick presents two filters with different behavior. In his paper, the first filter ends by Equation (23). The correction propagated through the gradient filter leads to a classic linear correction function. This filter is denoted Linear Madgwick. The second filter in his paper is based on fusion by (29). The constant correction step was used in this field for the first time. We call the second filter the Madgwick gradient filter. The comparison of the Linear Madgwick filter with its final version is discussed in the experimental part, Section 4.3.1.

The correction function is implemented in blocks “accCor” and “magCor” in Figure 1. If we use the constant correction step, the deviation angle is not included in the filter step. We can then save a small amount of computing power by omitting the computation of

α_{a}

and

α_{m}

in Equation (18) from the previous block.

It is clear that for a very small angle deviation, the constant correction step will lead to overshooting and thus to the zig–zag effect. This can be prevented by the use of the segmented function. A comparison of the effect of the tested correction functions is provided in the experimental part, see Section 4.3.2.

3.7. Fusion of Correction Quaternions (fuseCor)

The fusion step aims to create a single quaternion that represents a correction of the predicted reference frame towards the measurements. In Euclidean space, this is often achieved by convex combination. An equivalent operation in rotational space can be obtained numerically, by repetitive application of infinitesimal steps of alternating rotations toward the first and the second measurement. Since this is a computationally expensive operation, the most common approximation of this operation is to apply rotational multiplication of fused quaternions [21], which corresponds to a coarse approximation of the correct step. However, this simple approximation does not preserve commutativity and its accuracy is quickly decreasing with increasing angles and difference in direction of the fused quaternions.

As an alternative, we propose a simple analytical formula that approximates the numerical solution. We propose a three-step approximation. First, create a convex combination of the vectors

a_{c o r}

and

m_{c o r}

f_{c o r} = (\frac{β_{a}}{2} a_{c o r} + \frac{β_{m}}{2} m_{c o r}),

(22)

using weights

β_{a}

and

β_{m}

. Second, compute its curvature on the hypersphere

c = \sin c (norm (f_{c o r})),

(23)

where

\sin c (x) = sin (x) / x

is the unnormalized sinc function. Third, generate quaternion

\begin{matrix} q_{c o r} & = [\sqrt{1 - f_{c o r}^{⊤} f_{c o r} c^{2}}, f_{c o r} c], \end{matrix}

(24)

which is the final correction quaternion. For small rotations, the curvature c is approaching one, which allows simplifying (24) for the FSCF:

\begin{matrix} {\tilde{q}}_{c o r} & = [1, f_{c o r}], \end{matrix}

(25)

The accuracy of the proposed formula was tested in comparison with a numerical solution using a billion steps for each testing rotation. The accuracy of the tested methods for two rotations of perpendicular directions with the same value of the rotation angle is displayed in Figure 2 for increasing value of the rotation angle.

Note that the proposed Formula (24) is accurate for the whole tested interval of the rotation angles. The simplified formula is accurate for low values of the rotation angle but deteriorates over 30 degrees. The commonly used method of quaternion multiplication is the least accurate in this case.

3.8. Application of Correction Step (Correct)

The final filter block “correct” applies the correction step to the prediction by quaternion multiplication as

{\hat{q}}_{t} = q_{p r e d} \times q_{c o r} .

(26)

The FSCF filter contains few approximations of the mathematically correct SO(3) operations, (11), (21), and (25). The normalization of the estimated quaternion has to be done after each step of the filter. This is highly recommended also for SCF because of the numeric error integration.

4. Experimental Results

A comparison of the accuracy of AHRS filters is a problematic task since each filter has different behavior in different situations. Therefore, we prepared an experimental platform and recorded three different datasets with different characteristics.

We evaluated the performance of the filters in two steps. First, we calculated the rotation between the ground truth quaternion and the estimated quaternion

\begin{matrix} q_{d i f} & = q_{g t} \times q_{e s t}^{- 1} . \end{matrix}

(27)

Second, we evaluated the rotation angle of the difference

θ (q_{d i f})

using (7) in each time step of the experiment. Accuracy of the estimation was evaluated using two common measures, the mean absolute error (MAE) and the root mean square error (RMSE) of the rotation angle

θ (q_{d i f})

. We used MAE for graphical presentations since it is distinguishable for the large error range.

To minimize the potential of introducing an error in implementation, we preferred to compare the quality of our filter with the methods published with implemented code. Therefore, we chose the Madgwick’s complementary filter [21], the Valenti’s complementary filter [18], and the Guo’s Fast Kalman Filter [16]. However, none of the recently published filters with independent corrections from the magnetometer and accelerometer had available code. Therefore, we compared the methods with our implementation of Suh’s filter [26].

4.1. Data Acquisition

The experimental platform consisted of three perpendicular rods of lengths 36 cm to obtain better localization accuracy (Figure 3). The platform was equipped with low-cost sensor unit BMX055 from BOSCH Sensortec and nrf24l1s unit configured to act as a wireless transceiver. The data were captured by the second nrf24l1s unit operated as a wireless receiver connected to the Arduino UNO where they were mixed and logged with data coming from the VICON-460 optical positioning system. Accuracy of the positioning was studied in [29,30] confirming sub-millimeter accuracy of the position measurements, depending on operating conditions. Since even the worst-case error of 1 mm on the position of the rod implies a 0.16 degree error on its rotation, we considered this measurement to be the ground truth reference for the tested filters.

The recorded datasets and implementation of the SCF and FSCF are freely available (https://github.com/Josef4Sci/AHRS_Filter).

Three experiments were performed in the laboratory to capture different scenarios that can occur in practical applications. The first experiment was designed to contain relatively slow rotations, whereas the second experiment was designed to contain fast rotations. In the third experiment, the rotations are inhibited but the platform was subject to dynamic accelerations in different directions. Datasets recorded in each experiment were synthetically connected to create three different scenarios. The first scenario represents favorable conditions where only slow rotations are present, therefore only the dataset from the first experiment is used. This scenario is common e.g., in indoor drone applications. The second scenario represents more demanding conditions where both slow and fast rotations are common, as arise e.g., in virtual reality applications. This scenario is obtained by joining datasets from the first two experiments. The third scenario is the harshest conditions where rotations are disturbed by abrupt accelerations from arbitrary directions, as arise in inertial motion capture systems. This is obtained by joining data from all three experiments. Data from all three datasets are displayed in Figure 4.

4.2. Tuning of Filter Parameters

The tuning of filter parameters is essential for proper evaluation of their performance [31]. Therefore, we optimized the tuning parameters of all filters for each scenario and each performance measure (MAE, RMSE) independently. Since the problem is nonconvex, we needed a global optimization method. For this reason, many authors use a stochastic search, such as the particle swarm optimization [32], which may not be easily reproducible. Therefore, we investigated deterministic optimization methods. The conventional Nelder–Mead simplex search as implemented in Matlab was found to be unreliable. Therefore, we designed our version of the refined grid search [33]. Specifically, we started with an initial guess and designed a grid of values consisting of values of the initial guess multiplied by predefined coefficients. See supplementary material for details of implementation.

4.3. Influence of the Correction Function

The idea of the correction in its basic form was proposed in [21] and elaborated in a more advanced form in the proposed method. It was found to be a very important factor for the accuracy of different filters as is demonstrated in this Section. To obtain fair testing conditions, parameters of all tested filters were optimized for each test and each filter to get the best possible response. The errors of the filters are quite noisy in raw format, therefore we present them processed by the moving average filter with a window size of 40 samples for better readability (sampling rate about 100 Hz).

4.3.1. Correction Function in Madgwick Filters

The basic filter presented in [21] is using the linear function and will be denoted as Linear Madgwick. However, the main results presented in [21] are using the constant correction function and will be denoted as Madgwick in our comparison. The difference between those two versions is demonstrated in Figure 5 on the data from the first scenario. The version with constant correction function outperforms the linear filter by 58%. From now on, only the filter with constant correction function version will be used for tests.

4.3.2. Correction Function in SCF

Three shapes of the correction function proposed in Section 3.6 were tested: (i) the linear function, which has only one parameter per correction sensor; (ii) the constant function with one parameter per sensor as well; (iii) the segmented function with two parameters per correction sensor. To reduce the number of parameters in the segmented function, we tuned a single gain of the linear part for both sensors. This leads to a filter with three parameters for tuning.

The parameters of all correction functions were optimized for performance on scenario 3 and the results are displayed in Figure 6. The result shows that there is almost no difference in the accuracy of the filter with the constant correction function and its theoretically improved version, the segmented function. Note that the error of the filter with linear correction function grows significantly between 23 and 36 s (the part of heading change, Figure 6). Another major difference is also in acceleration disturbances. The influence of the correction function in fast rotations seems to be insignificant. In this part, the error is dominated by a peak between 60 and 70 s, which could be caused by the desynchronization between the reference and measured data.

The average error is summarized in Table 1.

Note that the impact of the segmented function on the error is low but tuning of the additional parameter is hard. Therefore, the constant correction function was used in the following tests.

4.4. Comparison on Selected Application Scenarios

The performance of the tested filters was evaluated for all three scenarios defined in Section 4.1. Parameters of all filters were tuned for each scenario independently to obtain the lowest average MAE or RMSE. The parameters are summarized in Table A1 in Appendix A. Since the SCF and FSCF have almost the same behavior, only the FSCF results are displayed in Figure 7, Figure 8 and Figure 9. Since the results of Suh’s filter [26] were significantly worse than other filters in the testing set, its estimates are displayed as transparent for better readability of the results of other filters. Results for the first scenario with only slow rotation are displayed in Figure 7 and summarized in Table 2. The performance of all filters is comparable in the first half of the dataset but differs in the second where the proposed filter has the lowest error.

The second scenario contains both slow and fast rotations. The results are summarized in Table 2 and displayed in detail in Figure 8. Note that the error during fast rotations (40–70 s) is much higher than that for the slow rotations and is dominated by a peak between 60–70 s. This peak can be also explained by deviation in the reference VICON data. However, since all filters have almost the same performance in this region, it has no impact on the results of the comparison. The filters were tuned for the best overall results in this scenario but the performance in the first part (slow rotations) is similar to results with parameters tuned only for the first scenario.

The last scenario complements slow and fast rotations by a sequence of dynamic accelerations to test robustness to disturbance. Once again, all filters were tuned for best overall performance in the whole data set. The results are summarized in Table 2 and analyzed in detail in Figure 9. Note that the proposed filter is significantly better than the competitors in the second part of the slow rotation data and in the dynamic acceleration data. In all other cases, it is comparable to the best of the remaining filters.

4.5. Cross-Validation Study

The previous experiments were optimized for best performance to obtain theoretical lower bound. In this section, we present the results of our investigation of the sensitivity of all tested methods to the tuning of their parameters. We performed a cross-validation study, in which we trained parameters of the methods on one part of the data and report their performance on the remaining data. Specifically, the introduced dataset is composed of the distinct parts: (i) slow rotation 0–40 s, that form scenario 1, (ii) fast rotations 40–72 s, and (iii) dynamic accelerations after 72 s. Since tuning of the parameter on the dynamic acceleration would provide degenerate behavior (the correct output is always zero), we optimize parameters of all methods only on one of the fast or slow parts and evaluate their performance on the remaining part of the data set, Table 3. The best performance is achieved for the proposed method, followed by the original Madgwick filter. This is an advantageous property of the gradient clipping that is insensitive to the absolute value of the error.

4.6. Computational Costs

The computational cost is an important factor for embedded systems containing MARG sensor combination. Processing a real-time data flow is the most common task for AHRS filters. The speed of filters can be compared via the number of floating-point operations in one filter step. The results for all tested methods are listed in Table 4.

The computational time of the Madgwick-type filters can be further reduced using implementation tricks techniques of [8] or the simplification of the problem [34]. While [8] achieves almost identical accuracy as [21], the simplification presented in [34] has a negative impact on accuracy, in our case 57% increase in MAE over [21] in scenario 3.

We propose a simplification of the magnetic reference in Section 3.4. This simplification allowed for the reduction of the computational cost of SCF and FSCS with negligible impact on accuracy. Replacing the proposed magnetic reference by the original reference of [21] lead to a 0.1% improvement of MAE on Scenario 3 of our experiments.

4.7. Summary of the Experimental Part

The benefit of using correct mathematical operations in SO(3) was found to have a negligible impact on the filter performance. Specifically, the improvement of the proposed method for quaternion fusion over the quaternion multiplication (Figure 2) was 0.3‰ for the method ignoring rotational curvature and 0.4‰ for the method with inverse curvature. However, it may become more important in applications with lower sampling frequency.

A more important factor in filter performance is the choice of the correction function, as studied in Section 4.3.2. The constant correction step was found to be a good choice for standard applications with MARG sensor combination. Methods that use the correction function (such as the constant correction step) are also less sensitive to tuning parameters and perform well in cross-validation study.

However, the main contribution to the good performance of the proposed filter is the application of the correction function independently to data from accelerometer and magnetometer before their fusion. In our tests, the optimal weight of the magnetometer was approximately 10 times smaller than the weight of the accelerometer. This indicates that filters with common weight for both accelerometer and magnetometer are sub-optimal in real applications.

The proposed filters were found to be more accurate than any of its competitors in all tests. Specifically, the mean absolute error of the proposed filter was 77% of the second-best filter in scenario 1 (slow rotations), 93% of the second-best in scenario 2 (slow and fast rotations), and 86% in scenario 3 (slow and fast rotations, and dynamic acceleration).

The biggest advantage of the proposed filter is in its accuracy for slower rotations and dynamic accelerations. Nevertheless, the improvement for faster rotations is also significant.

5. Conclusions

A novel predictor—corrector AHRS filter using separate correction steps for magnetometer and accelerometer (SCF) and its faster version (FSCF) is proposed. Both filters have lower computational cost than other well-known methods in this field. Although both filters have simple structures, it was demonstrated that they are more accurate than their competitors. The most visible improvement was on a scenario with slow rotations (77% mean absolute error of the second-best filter) but it improves on the dynamic acceleration as well as fast rotations. The key difference from previous approaches is the application of correction function independently to data from accelerometer and magnetometer before their fusing. This opens possibilities for future research, e.g., on-line adaptation of the weights. We also provide our testing dataset for open access for better reproducibility of our results.

Author Contributions

Data curation, J.J.; methodology, J.J.; project administration, A.H.; software, J.J.; supervision, A.H.; validation, J.J.; writing—review and editing, V.Š. All authors have read and agreed to the published version of the manuscript.

Funding

This research was supported by the Ministry of Education, Youth, and Sports of the Czech Republic under the project OP VVV Electrical Engineering Technologies with High-Level of Embedded Intelligence CZ.02.1.01/0.0/0.0/18_069/0009855 and by the UWB Student grant project no. SGS-2018-016.

Acknowledgments

Support of the VICON laboratory of Faculty of Applied Sciences of the University of West Bohemia is gratefully acknowledged. We are grateful to anonymous reviewers for comments that helped to clarify presentation of our work.

Conflicts of Interest

The authors declare no conflict of interest.

Appendix A. Experimental Details

In this section, we provide details of the experimental setting. Final values of the filter parameters obtained by optimization are summarized in Table A1.

Table A1. Review of optimizes parameters for all tested filters, optimization criteria (the mean absolute error (MAE) and the root mean square error (RMSE)), and test scenarios.

		MAE
Filter Type	Parameter	Scenario 1	Scenario 2	Scenario 3
Madgwick	$β$	0.0092	0.0101	0.0155
Valenti	[ $μ_{a c c}, μ_{m a g}$ ]	[0.0076; −2 × 10 $^{- 4}$ ]	[0.0056; 0.0028]	[0.0024; 0]
Suh	[rg, ra, rm]	[11.56; 1.39 × 10 $^{- 19}$ ; 0.0057]	[0.1548; 5.19 × 10 $^{- 10}$ ; 3.84 × 10 $^{- 6}$ ]	[0.0756; 1.75 × 10 $^{- 16}$ ; 1.57 × 10 $^{- 7}$ ]
Guo ( $Σ_{g y r} = 1$ )	[ $Σ_{a c c}, Σ_{m a g}$ ]	[0.247; 368.4]	[0.405; 553.1]	[4.108; 513.3]
SCF and FSCF	[ $λ_{a}, λ_{m}$ ]	[0.0028; 0.0001]	[0.0021; 0.0001]	[0.0016; 0.0001]
		RMSE
		Scenario 1	Scenario 2	Scenario 3
Madgwick	$β$	0.0113	0.0160	0.0170
Valenti	[ $μ_{a c c}, μ_{m a g}$ ]	[0.009; −2 × 10 $^{- 4}$ ]	[0.0027; 1.4 × 10 $^{- 5}$ ]	[0.0026; 0]
Suh	[rg, ra, rm]	[1.009; 5.3 × 10 $^{- 5}$ ; 26 × 10 $^{- 4}$ ]	[2.125; 2.7 × 10 $^{- 4}$ ; 9.4 × 10 $^{- 5}$ ]	[5.058; 2.1 × 10 $^{- 4}$ ; 1.3 × 10 $^{- 5}$ ]
Guo ( $Σ_{g y r} = 1$ )	[ $Σ_{a c c}, Σ_{m a g}$ ]	[0.3726; 162.4]	[0.873; 484.4]	[3.377; 511.3]
SCF and FSCF	[ $λ_{a}, λ_{m}$ ]	[0.0028; 0.0001]	[0.0014; 0.0001]	[0.0013; 0.0001]

References

Wahba, G. A Least-Squares Estimate of Spacecraft Attitude. SIAM Rev. 1965, VII, 409. [Google Scholar] [CrossRef]
Shuster, M.; Oh, S. Three-axis attitude determination from vector observations. J. Guid. Control 1981, 4, 70–77. [Google Scholar] [CrossRef]
Mortari, D. ESOQ: A closed-form solution to the Wahba problem. J. Astronaut. Sci. 1997, 45, 195–204. [Google Scholar]
Markley, F.L.; Mortari, D. Quaternion attitude estimation using vector observations. J. Astronaut. Sci. 2000, 48, 359–380. [Google Scholar]
Mims, J.H.; Farrell, J.L. Synthetic aperture imaging with maneuvers. IEEE Trans. Aerosp. Electron. Syst. 1972, 4, 410–418. [Google Scholar] [CrossRef]
Gebre-Egziabher, D.; Elkaim, G.H.; Powell, J.D.; Parkinson, B.W. A gyro-free quaternion-based attitude determination system suitable for implementation using low cost sensors. In Proceedings of the Position Location and Navigation Symposium, San Diego, CA, USA, 13–16 March 2000; pp. 185–192. [Google Scholar]
Gebre-Egziabher, D.; Hayward, R.C.; Powell, J.D. Design of multi-sensor attitude determination systems. IEEE Trans. Aerosp. Electron. Syst. 2004, 40, 627–649. [Google Scholar] [CrossRef]
Admiraal, M.; Wilson, S.; Vaidyanathan, R. Improved formulation of the IMU and MARG orientation gradient descent algorithm for motion tracking in human-machine interfaces. In Proceedings of the 2017 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), Daegu, Korea, 16–18 November 2017; pp. 403–410. [Google Scholar]
Zmitri, M.; Fourati, H.; Vuillerme, N. Human Activities and Postures Recognition: From Inertial Measurements to Quaternion-Based Approaches. Sensors 2019, 19, 4058. [Google Scholar] [CrossRef] [Green Version]
Sabatini, A.M. Quaternion-based extended kalman filter for determining orientation by inertial and magnetic sensing. J. Sens. 2006, 53, 1346–1356. [Google Scholar] [CrossRef]
Zhang, M.; Li, K.; Hu, B.; Meng, C. Comparison of Kalman Filters for Inertial Integrated Navigation. Sensors 2019, 19, 1426. [Google Scholar] [CrossRef] [Green Version]
Chiella, A.C.; Teixeira, B.O.; Pereira, G.A. Quaternion-Based Robust Attitude Estimation Using an Adaptive Unscented Kalman Filter. Sensors 2019, 19, 2372. [Google Scholar] [CrossRef] [Green Version]
Odry, Á.; Kecskes, I.; Sarcevic, P.; Vizvari, Z.; Toth, A.; Odry, P. A Novel Fuzzy-Adaptive Extended Kalman Filter for Real-Time Attitude Estimation of Mobile Robots. Sensors 2020, 20, 803. [Google Scholar] [CrossRef] [PubMed] [Green Version]
Kang, C.W.; Kim, H.J.; Park, C.G. A human motion tracking algorithm using adaptive EKF based on Markov chain. IEEE Sens. J. 2016, 16, 8953–8962. [Google Scholar] [CrossRef]
Narasimhappa, M.; Mahindrakar, A.D.; Guizilini, V.C.; Terra, M.H.; Sabat, S.L. MEMS Based IMU Drift Minimization: Sage Husa Adaptive Robust Kalman Filtering. IEEE Sens. J. 2019, 20, 250–260. [Google Scholar] [CrossRef]
Guo, S.; Wu, J.; Wang, Z.; Qian, J. Novel MARG-Sensor Orientation Estimation Algorithm Using Fast Kalman Filter. J. Sens. 2017, 1, 8542153. [Google Scholar] [CrossRef] [Green Version]
Mahony, R.; Hamel, T.; Pflimlin, J.M. Complementary filter design on the special orthogonal group SO (3). In Proceedings of the IEEE Conference on Decision and Control (CDC-ECC’05), Seville, Spain, 15 December 2005; pp. 1477–1484. [Google Scholar]
Valenti, R.; Dryanovski, I.; Xiao, J. Keeping a good attitude: A quaternion-based orientation filter for IMUs and MARGs. Sensors 2015, 15, 19302–19330. [Google Scholar] [CrossRef] [PubMed]
Wu, J.; Zhou, Z.; Chen, J.; Fourati, H.; Li, R. Fast complementary filter for attitude estimation using low-cost MARG sensors. IEEE Sens. J. 2016, 16, 6697–7007. [Google Scholar] [CrossRef]
Wu, J.; Zhou, Z.; Fourati, H.; Cheng, Y. A super fast attitude determination algorithm for consumer-level accelerometer and magnetometer. IEEE Trans. Consum. Electron. 2018, 64, 375–381. [Google Scholar] [CrossRef]
Madgwick, S.O.; Harrison, A.J.; Vaidyanathan, R. Estimation of IMU and MARG orientation using a gradient descent algorithm. In Proceedings of the Rehabilitation Robotics (ICORR) 2011 IEEE International Conference, Zurich, Switzerland, 29 June–1 July 2011; pp. 1–7. [Google Scholar]
Liu, F.; Li, J.; Wang, H.; Liu, C. An improved quaternion Gauss–Newton algorithm for attitude determination using magnetometer and accelerometer. Chin. J. Aeronaut. 2014, 27, 986–993. [Google Scholar] [CrossRef] [Green Version]
Goodfellow, I.; Bengio, Y.; Courville, A. Deep Learning; MIT Press: Cambridge, MA, USA, 2016. [Google Scholar]
Zhang, J.; He, T.; Sra, S.; Jadbabaie, A. Why Gradient Clipping Accelerates Training: A Theoretical Justification for Adaptivity. In Proceedings of the International Conference on Learning Representations, New Orleans, LA, USA, 6–9 May 2019. [Google Scholar]
Feng, K.; Li, J.; Zhang, X.; Shen, C.; Bi, Y.; Zheng, T.; Liu, J. A new quaternion-based Kalman filter for real-time attitude estimation using the two-step geometrically-intuitive correction algorithm. Sensors 2017, 17, 2146. [Google Scholar] [CrossRef] [PubMed] [Green Version]
Suh, Y.S. Simple-Structured Quaternion Estimator Separating Inertial and Magnetic Sensor Effects. IEEE Trans. Aerosp. Electron. Syst. 2019, 55, 2698–2706. [Google Scholar] [CrossRef]
Fan, B.; Li, Q.; Liu, T. Improving the accuracy of wearable sensor orientation using a two-step complementary filter with state machine-based adaptive strategy. Meas. Sci. Technol. 2018, 29, 115104. [Google Scholar] [CrossRef]
Markley, F.L.; Crassidis, J.L. Fundamentals of Spacecraft Attitude Determination and Control; Springer: Berlin, Germany, 2014; Volume 33. [Google Scholar]
Windolf, M.; Götzen, N.; Morlock, M. Systematic accuracy and precision analysis of video motion capturing systems—Exemplified on the Vicon-460 system. J. Biomech. 2008, 41, 2776–2780. [Google Scholar] [CrossRef]
Merriaux, P.; Dupuis, Y.; Boutteau, R.; Vasseur, P.; Savatier, X. A study of vicon system positioning performance. Sensors 2017, 17, 1591. [Google Scholar] [CrossRef] [PubMed]
Mazza, C.; Donati, M.; McCamley, J.; Picerno, P.; Cappozzo, A. An optimized Kalman filter for the estimate of trunk orientation from inertial sensors data during treadmill walking. Gait Posture 2012, 35, 138–142. [Google Scholar] [CrossRef]
Odry, Á.; Fuller, R.; Rudas, I.J.; Odry, P. Kalman filter for mobile-robot attitude estimation: Novel optimized and adaptive solutions. Mech. Syst. Signal Process. 2018, 110, 569–589. [Google Scholar] [CrossRef]
Coope, I.D.; Price, C.J. On the convergence of grid-based methods for unconstrained optimization. SIAM J. Optim. 2001, 11, 859–869. [Google Scholar] [CrossRef] [Green Version]
Wilson, S.; Eberle, H.; Hayashi, Y.; Madgwick, S.O.; McGregor, A.; Jing, X.; Vaidyanathan, R. Formulation of a new gradient descent MARG orientation algorithm: Case study on robot teleoperation. Mech. Syst. Signal Process. 2019, 130, 183–200. [Google Scholar] [CrossRef]

Figure 1. Block diagram of the Madgwick magnetic angular rate and gravity sensor combination (MARG) filter (left) and the proposed separated correction filter (SCF) (right). Yellow blocks denote prediction from the gyroscope; Blue blocks denote correction from the magnetometer; Red blocks the accelerometer correction, and Green is fusion of the correction steps and its application to the predicted values. Detailed description of blocks of the proposed filter is provided in Section 3, namely Section 3.1 (qDot), Section 3.2 (qPred), Section 3.3 (aRef, accPred), Section 3.4 (mRef, magPred), Section 3.5 (magDev, accDev), Section 3.6 (magCor, accCor), Section 3.7 (fuseCor), and Section 3.8 (correct). Tuning parameters of the proposed filter are

λ_{m}

and

λ_{a}

λ_{m}

and

λ_{a}

Figure 2. Accuracy of quaternion fusion of two rotations of perpendicular directions with the same value of the rotation angle for the quaternion multiplication (blue), the proposed method with inverse quadrature (24) (yellow), and the simplified method (25) ignoring the curvature (red). The error is computed as a deviation from the numerical solution with billion steps.

Figure 3. The measuring platform in the VICON laboratory.

Figure 4. The measured data from all three experiments. The first three rows display data from the MARG sensor: all three axes,

x, y, z

, of the accelerometer, magnetometer, and gyroscope, respectively. The measured positions of the points on the platform from the VICON system were converted to the reference quaternion that is displayed via its vector components,

w, x, y, z

(3) in the bottom row. The slow rotations occur between 0–40 s, fast rotations between 40–72 s, and dynamic accelerations after 72 s.

Figure 4. The measured data from all three experiments. The first three rows display data from the MARG sensor: all three axes,

x, y, z

w, x, y, z

(3) in the bottom row. The slow rotations occur between 0–40 s, fast rotations between 40–72 s, and dynamic accelerations after 72 s.

Figure 5. Comparison of accuracy of Madgwick’s filters for two choices of the correction function on the data from the first scenario. Parameters of all filters were tuned for the best overall performance.

Figure 6. Accuracy of the separated correction filter (SCF) for scenario 3 and three different choices of the correction function: Linear, constant, and segmented. Parameters of all filters were tuned for the best overall performance.

Figure 7. Error of the tested filters for data from the first scenario. Parameters of all filters were tuned for best overall (scenario 3) performance.

Figure 8. Error of tested filters for data from the second scenario. Parameters of all filters were tuned for best overall (scenario 3) performance.

Figure 9. Error of all tested filters for the data from the third scenario (top row): slow rotations (left), fast rotations (middle), and dynamic acceleration (right). Parameters of all filters were tuned for the best overall performance. Quaternion estimated by the fast separated correction filter (FSCF) is displayed for illustration (bottom row).

Table 1. Comparison of the proposed filter with different correction function shapes.

Shape	MAE Error (Deg)
Linear	5.19
Constant	3.37
Segmented	3.37

Table 2. Estimation errors of tested filters for all tested scenarios. Parameters of all filters were optimized for the best performance.

	MAE [Deg]			RMSE [Deg]
Filter Type	Scenario 1	Scenario 2	Scenario 3	Scenario 1	Scenario 2	Scenario 3
Madgwick	2.66	4.63	3.90	3.48	6.61	5.91
Valenti	2.84	5.91	4.51	3.82	7.08	6.39
Suh	5.29	10.68	11.54	6.53	15.01	15.51
Guo	2.57	4.35	3.94	3.49	6.41	5.82
SCF (ours)	1.97	4.06	3.37	3.01	6.24	5.57
FSCF (ours)	1.97	4.06	3.37	3,01	6.24	5.57

Table 3. Accuracy of the tested methods on testing data set. Parameters of all methods were trained either on the slow or fast rotation part of the data set. Reported accuracy is measured on the testing data, which is the remaining part of the dataset.

	MAE [Deg]		RMSE [Deg]
Filter Type	Trained on Fast	Trained on Slow	Trained on Fast	Trained on Slow
Madgwick	3.30	5.62	4.29	7.76
Valenti	4.84	13.93	5.85	15.52
Suh	24.10	34.11	42.29	48.18
Guo	3.37	6.72	4.13	8.52
SCF (ours)	3.20	5.29	3.91	7.51
FSCF (ours)	3.20	5.29	3.91	7.51

Table 4. The computational cost for each filter in terms of floating-point operations.

Filter Type	Filter FLOPs
Madgwick	287
Valenti	244
Suh	352
Guo	≈1000
SCF	179
FSCF	160

© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

Share and Cite

MDPI and ACS Style

Justa, J.; Šmídl, V.; Hamáček, A. Fast AHRS Filter for Accelerometer, Magnetometer, and Gyroscope Combination with Separated Sensor Corrections. Sensors 2020, 20, 3824. https://doi.org/10.3390/s20143824

AMA Style

Justa J, Šmídl V, Hamáček A. Fast AHRS Filter for Accelerometer, Magnetometer, and Gyroscope Combination with Separated Sensor Corrections. Sensors. 2020; 20(14):3824. https://doi.org/10.3390/s20143824

Chicago/Turabian Style

Justa, Josef, Václav Šmídl, and Aleš Hamáček. 2020. "Fast AHRS Filter for Accelerometer, Magnetometer, and Gyroscope Combination with Separated Sensor Corrections" Sensors 20, no. 14: 3824. https://doi.org/10.3390/s20143824

APA Style

Justa, J., Šmídl, V., & Hamáček, A. (2020). Fast AHRS Filter for Accelerometer, Magnetometer, and Gyroscope Combination with Separated Sensor Corrections. Sensors, 20(14), 3824. https://doi.org/10.3390/s20143824

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Menu

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

Abstract

1. Introduction

2. Important Rotation Operations

3. Filter Implementation

3.1. Dynamic Rotation Change (qDot)

3.2. Prediction Integration (qPred)

3.3. Accelerometer Reference Vector (aRef, accPred)

3.4. Magnetometer Reference Vector (mRef, magPred)

3.5. Deviation of Prediction and Measurement (magDev, accDev)

3.6. Correction Step Determination (magCor, accCor)

3.7. Fusion of Correction Quaternions (fuseCor)

3.8. Application of Correction Step (Correct)

4. Experimental Results

4.1. Data Acquisition

4.2. Tuning of Filter Parameters

4.3. Influence of the Correction Function

4.3.1. Correction Function in Madgwick Filters

4.3.2. Correction Function in SCF

4.4. Comparison on Selected Application Scenarios

4.5. Cross-Validation Study

4.6. Computational Costs

4.7. Summary of the Experimental Part

5. Conclusions

Author Contributions

Funding

Acknowledgments

Conflicts of Interest

Appendix A. Experimental Details

References

Share and Cite

Article Metrics

Article Access Statistics

Further Information

Guidelines

MDPI Initiatives

Follow MDPI