Design for an Orientation Algorithm of Ahrs Based on Gain Scheduling

1. Introduction

In recent years, the performance of Micro-Electro-Mechanical Systems (MEMS) has significantly improved. Due to their low cost, small size, and low energy consumption, their applications have increasingly proliferated [1]. One notable application is the construction of the Orientation and Heading Reference System (AHRS) [2]. AHRS, by measuring the orientation of moving objects, finds applications in numerous fields, including autonomous vehicles, Unmanned Aerial Vehicles (UAVs), Autonomous Underwater Vehicles (AUVs), human motion tracking, and autonomous robot control [3,4,5,6,7]. However, various interferences in practical applications affect the final system’s orientation measurement accuracy. Consequently, developing a robust anti-interference AHRS orientation algorithm has become a significant research topic [8,9].

In the process of sensor data fusion in AHRS, the commonly used methods are the Extended Kalman Filter (EKF) [10] and complementary filtering [11]. The EKF is widely employed in various aerospace projects. However, the complexity of the EKF algorithm is higher than that of complementary filtering, which increases the overall system cost [12]. In many practical applications, simpler filtering and compensation algorithms are preferred. Mahony [13] et al. proposed a nonlinear complementary filtering algorithm for fusing measurements from different sensors. Madgwick [14] et al. introduced a gradient descent method to estimate the system’s orientation. This method uses a linear complementary filter algorithm to combine the observed orientation from the system with the estimated orientation from the gyroscope to suppress drift. These fusion algorithms leverage the complementary characteristics of sensors to achieve more accurate and less noisy orientation and heading estimates. However, in high dynamic or magnetically disturbed environments, the apparent acceleration-induced horizontal orientation errors and magnetic field interference-induced heading errors can lead to inaccurate Euler angles calculations [15,16,17,18,19].

To address the challenges posed by acceleration and magnetic field interference, this paper proposes a gain-scheduled gradient descent algorithm based on the traditional gradient descent method. The gain is determined by a switching architecture based on magnetometer and accelerometer measurements, ensuring robust performance even in the presence of magnetic interference or sudden accelerations. The proposed scheme first dynamically adjusts the roll and pitch angle gains based on accelerometer measurements, then dynamically adjusts the yaw angle gain based on magnetometer measurements. Experiments conducted under acceleration and magnetic interference environments demonstrate that the gain-scheduled gradient descent method achieves more stable orientation resolution performance in complex usage scenarios.

2. Overview of Orientation and Heading Reference Systems

2.1. Principles of AHRS

The sensors comprising an Orientation and Heading Reference System (AHRS) include MEMS-based triaxial gyroscopes, MEMS-based triaxial accelerometers, and triaxial magnetometers. Within the AHRS, the initial step involves reading angular velocity, acceleration, and magnetic field strength data from each axis of the sensor units. This angular velocity data is subsequently processed to obtain changes in orientation. By combining this with the previous orientation of the vehicle, a new predicted Euler angles is derived.

However, errors such as gyroscopic bias, scale factor error, cross-axis coupling error, and noise inherent in the gyroscopes can cause the integrated orientation results to drift from the true values over time. Therefore, data fusion techniques are commonly employed to calculate Euler angles using accelerometer and magnetometer measurements, which compensate for the gyroscope-derived Euler angles and suppress gyroscopic drift.

The Euler angles derived from the gyroscopes can also provide boundary conditions for the accelerometer and magnetometer-derived Euler angles, particularly under high dynamic conditions or magnetic interference. The workflow of AHRS is illustrated in Figure 1.

2.2. Sensor Data Fusion

In this paper, the navigation frame is defined as the East-North-Up (O-ENU) frame. The X-axis of the body frame points to the right in the horizontal plane of the vehicle, the Y-axis points forward in the horizontal plane, and the Z-axis points vertically upward from the horizontal plane of the vehicle, as shown in Figure 2. To prevent singularities and reduce the overall computational load of the algorithm, quaternion-based orientation update method is adopted. The quaternion describing this orientation, $q_b^n$, is given by Equation (1).

$$q_b^n={\left[\begin{array}{cccc}{q}_1& q_2& q_3& q_4\end{array}\right]}^T$$

(1)

The solving process of the traditional gradient descent method is as follows: first, the relationship between the angular velocity and quaternion derivative is calculated through numerical integration to determine the vehicle’s orientation. Then, the measurements from the magnetometer and accelerometer are taken as observation vectors, and the orientation is estimated using the gradient descent method. Finally, the orientation quaternion obtained from gyroscope integration and the orientation quaternion derived from the observation vectors are combined in a complementary filter to accurately estimate the vehicle’s orientation.

The angular rate measured by the gyroscope is denoted as ${\omega }_{ib}^b$, which can be expressed in the form of Equation (2). Since MEMS gyroscopes are used in this paper, the Earth’s rotation rate ${\omega }_{ie}^b$ and the coupling angular velocity between the navigation frame and the earth frame ${\omega }_{en}^b$are relatively small, so the measured angular rate can be approximated as ${\omega }_{nb}^b$. The relationship between the gyroscope-measured angular rate and the quaternion derivative ${\dot{q}}_b^n$ can be represented by Equations (3) and (4).

$${\omega }_{ib}^b={\omega }_{ie}^b+{\omega }_{en}^b+{\omega }_{nb}^b$$

(2)

$${\omega }_{nb}^b={\left[\begin{array}{ccc}{\omega }_x& {\omega }_y& {\omega }_z\end{array}\right]}^T$$

(3)

$${\dot{q}}_b^n=\frac{1}{2}{q}_b^n\otimes \left[\begin{array}{c}0\\ {\omega }_{nb}^b\end{array}\right]$$

(4)

Under the assumption of a known initial orientation, the orientation of the body frame relative to the navigation frame at time t can be obtained by integrating the quaternion derivative equation, as shown in Equations (5) and (6):

$${\dot{q}}_b^n(\omega ,t)=\frac{1}{2}{q}_b^n(est,t-1)\otimes \left[\begin{array}{c}0\\ {\omega }_{nb}^b(t)\end{array}\right]$$

(5)

$$q_b^n(\omega ,t)=q_b^n(est,t-1)+{\dot{q}}_b^n(\omega ,t)\Delta t$$

(6)

In the above equations, $q_b^n(\omega ,t)$ is the prior estimate quaternion of the orientation of the body frame relative to the navigation frame obtained from gyroscope data at time t, ${\dot{q}}_b^n(\omega ,t)$ is the derivative of the orientation quaternion measured by the gyroscope at time t, ${\omega }_{nb}^b(t)$ is the angular rate measured by the gyroscope at time t, $\Delta t$ is the gyroscope sampling period, and $q_b^n(est,t-1)$ is the posterior estimate of the orientation at the previous time step.

Using the Earth’s gravity vector and the geomagnetic vector as observation vectors, the value of the gradient $\nabla f$ can be calculated. When the Earth’s gravity vector is used as an observation vector, its magnitude is ${\widehat{g}}^n=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]$, and its direction is opposite to the navigation frame’s z-axis. Normalizing the accelerometer measurement ${\widehat{a}}^b=\left[\begin{array}{ccc}{a}_x& a_y& a_z\end{array}\right]$, we can obtain Equations (7) and (8).

$$f_g\left(q_b^n,{\widehat{a}}^b\right)=\left[\begin{array}{c}2\left(q_2{q}_4-q_1{q}_3\right)-a_x\\ 2\left(q_1{q}_2+q_3{q}_4\right)-a_y\\ (1-2q_2{}^2-q_3{}^2)-a_z\end{array}\right]$$

(7)

$$J_g(q_b^n)=\left[\begin{array}{cccc}\begin{array}{c}-2q_3\\ 2q_2\\ 0\end{array}& \begin{array}{c}2q_4\\ 2q_1\\ -4q_2\end{array}& \begin{array}{c}-2q_1\\ 2q_4\\ -4q_3\end{array}& \begin{array}{c}2q_2\\ 2q_3\\ 0\end{array}\end{array}\right]$$

(8)

When using the geomagnetic vector as an observation vector, the geomagnetic vector can be considered to have components on a horizontal axis and a vertical axis, with a magnitude of ${\widehat{b}}^n=\left[\begin{array}{ccc}{b}_x& 0& b_z\end{array}\right]$. Normalizing the magnetometer measurement ${\widehat{m}}^b=\left[\begin{array}{ccc}{h}_x& h_y& h_z\end{array}\right]$, we can obtain Equations (9) and (10).

$$f_b\left(q_b^n,{\widehat{b}}^n,{\widehat{m}}^b\right)=\left[\begin{array}{c}{b}_x\left(1-2q_3{}^2-2q_4{}^2\right)+2b_z\left(q_2{q}_4-q_1{q}_3\right)-m_x\\ 2b_x(q_2{q}_3-q_1{q}_4)+2b_z(q_1{q}_2+q_3{q}_4)-m_y\\ 2b_x(q_1{q}_3+q_2{q}_4)+b_z(1-2q_2{}^2-2q_3{}^2)-m_z\end{array}\right]$$

(9)

$$J_b(q_b^n,{\widehat{b}}^n)=\left[\begin{array}{cccc}-2b_z{q}_3& 2b_z{q}_4& -4b_x{q}_3-2b_z{q}_1& -4b_x{q}_4+2b_z{q}_2\\ -2b_x{q}_4+2b_z{q}_2& 2b_x{q}_3+2b_z{q}_1& 2b_x{q}_2+2b_z{q}_4& -2b_x{q}_1+2b_z{q}_3\\ 2b_x{q}_3& 2b_x{q}_4-4b_z{q}_2& 2b_x{q}_1-4b_z{q}_3& 2b_x{q}_2\end{array}\right]$$

(10)

The geomagnetic vector measured at time t, ${\widehat{h}}^n(t)$, can be obtained by rotating the normalized magnetometer measurement ${\widehat{m}}^b(t)$ using the posterior estimate of the orientation from the previous time step ${\widehat{q}}_b^n(est,t-1)$, as shown in Equation (11). If the measured geomagnetic vector is incorrectly inclined, it can be corrected to the same inclination plane, as shown in Equation (12).

$${\widehat{h}}^n(t)=[\begin{array}{cccc}0& h_x& h_y& h_z\end{array}]=q_b^n(est,t-1)\otimes {\widehat{m}}^b(t)\otimes q_b^n{(est,t-1)}^T$$

(11)

$$b^n(t)=[\begin{array}{cccc}0& \sqrt{h_x{}^2+h_y{}^2}& 0& h_z\end{array}]$$

(12)

At this point, using the Earth’s gravity vector as an observation vector can suppress the divergence of the pitch and roll angles. To avoid the remaining yaw angle divergence, it is necessary to use both the Earth’s gravity vector and the geomagnetic vector as observation vectors. In this case, the objective function and the Jacobian matrix are given by Equations (13) and (14).

$$f_{g,b}(q_b^n,{\widehat{a}}^b,{\widehat{b}}^n,{\widehat{m}}^b)=\left[\begin{array}{c}{f}_g({\widehat{q}}_b^n,{\widehat{a}}^b)\\ f_b({\widehat{q}}_b^n,{\widehat{b}}^n,{\widehat{m}}^b)\end{array}\right]$$

(13)

$$J_{g,b}({\widehat{q}}_b^n,{\widehat{b}}^n)=\left[\begin{array}{c}{J}_g^T\left({\widehat{q}}_b^n\right)\\ J_b^T\left({\widehat{q}}_b^n,{\widehat{b}}^n\right)\end{array}\right]$$

(14)

According to the gradient calculation formula, at this point, $\nabla f$ is given by Equation (15).

$$\nabla f_{g,b}=J_{g,b}^T{f}_{g,b}$$

(15)

Based on the gradient descent method, the gradient descent estimate at time t, ${\widehat{q}}_b^n(\nabla ,t)$, can be calculated from the posterior estimate of the orientation at the previous time step $q_b^n(est,t-1)$ through a single iteration, as shown in Equation (16).

$${\widehat{q}}_b^n(\nabla ,t)=q_b^n(est,t-1)-{\mu }_t\frac{\nabla f}{\Vert \nabla f\Vert }$$

(16)

By fusing the orientation quaternions $q_b^n(\omega ,t)$ and ${\widehat{q}}_b^n(\nabla ,t)$ obtained from Equations (6) and (16) respectively, the posterior estimate of the orientation quaternion at the current time t is derived. The fusion is performed as shown in Equation (17), where ${\gamma }_t$ is the weight applied to the orientation quaternion.

$$q_b^n\left(est,t\right)={\gamma }_t{\widehat{q}}_b^n(\nabla ,t)+(1-{\gamma }_t)q_b^n(\omega ,t),0\le {\gamma }_t\le 1$$

(17)

The optimal value of ${\gamma }_t$ is defined to ensure that the weighted divergence of $q_b^n(\omega ,t)$ equals the weighted convergence of ${\widehat{q}}_b^n(\nabla ,t)$. This is defined by Equation (18), where β is the divergence rate of $q_b^n(\omega ,t)$, representing the magnitude of the quaternion derivative corresponding to the gyroscope measurement error.

$$\left(1-{\gamma }_t\right)\beta ={\gamma }_t\frac{{\mu }_t}{\Delta t}$$

(18)

Given that β is generally large, Equation (16) has an approximate expression as shown in Equation (19).

$${\widehat{q}}_b^n(\nabla ,t)\approx -{\mu }_t\frac{\nabla f}{\Vert \nabla f\Vert }$$

(19)

Substituting Equations (6) and (19) into Equation (17) yields Equations (20) and (21).

$$q_b^n\left(est,t\right)=q_b^n(est,t-1)+{\dot{q}}_b^n\left(est,t\right)\Delta t$$

(20)

$${\dot{q}}_b^n\left(est,t\right)={\dot{q}}_b^n(\omega ,t)-\beta \frac{\nabla f}{\Vert \nabla f\Vert }$$

(21)

Figure 3 illustrates the block diagram of the implementation of the filtering algorithm.

3. Design of Gain-Scheduled Madgwick Algorithm

3.1. Issues with Traditional Madgwick Algorithm

For accelerometers, if the vehicle rotates, Coriolis acceleration due to the internal lever arm will be produced. This results in the accelerometer’s output containing components other than the Earth’s gravitational acceleration, thus affecting orientation measurement. Furthermore, changes in the apparent acceleration of the vehicle are another potential source of error. This means that the orientation derived from the accelerometer output is not reliable under dynamic conditions.

For magnetometers, metal structures and electrical equipment in the surrounding environment can produce magnetic interference, affecting the magnetometer’s performance. When the vehicle is near strong magnetic fields (such as power lines or railways), the magnetometer may become saturated, leading to errors. Such errors can reduce the accuracy of yaw angle computation or even produce incorrect results, leading to misalignment in updates.

In low dynamic conditions with minimal magnetic interference, the traditional gradient descent method can effectively suppress the divergence of the orientation angles computed by the gyroscope. However, when the magnetometer is affected by magnetic interference or the accelerometer experiences dynamic acceleration or platform vibrations, using the traditional gradient descent method for orientation fusion can result in significant errors, causing system misalignment.

3.2. Identifying Accelerometer Distortion

The accelerometer may be affected by vibrations from the platform on which it is mounted. In some applications, when the vehicle experiences dynamic acceleration, the accelerometer measures not only gravitational acceleration but also the vehicle’s motion acceleration. When the vehicle is in a high dynamic state, the orientation derived from the accelerometer becomes highly inaccurate, affecting the subsequent roll and pitch angles in orientation fusion.

To address the issue of accelerometer distortion, a gain factor is introduced in the roll and pitch channels. The determination of the gain factor is based on the level of acceleration perceived by the accelerometer to achieve optimal performance.

In the proposed scheme, different gains are applied to the gyroscope and accelerometer solutions based on the acceleration level (non-acceleration, low acceleration, and high acceleration modes) determined from the accelerometer measurements, as defined by Equation (25):

$$\alpha (k)=\left|\sqrt{a_x{(k)}^2+a_y{(k)}^2+a_z{(k)}^2}-g\right|$$

(22)

The logic for switching the actual gain is as follows:

Non-acceleration mode: $\alpha (k)<0.010g$, In this mode, the gravity vector measured by the accelerometer is observable, providing a good estimate of roll and pitch angles, with the accelerometer being given most of the weight during computation.

Low-acceleration mode: $0.010g<\alpha (k)<0.5g$, In this mode, the uncertainty in the orientation estimated by the accelerometer must be considered, with the gyroscope given a higher weight during computation.

High-acceleration mode: $\alpha (k)>0.5g$, In this mode, the system is in a high dynamic state. The roll and pitch orientations derived from the accelerometer measurements are highly erroneous, requiring the accelerometer gain to be set to 0, effectively turning off the accelerometer feedback, while the gyroscope is given full weight.

The gain scheduling logic for the roll and pitch angles derived from the accelerometer is illustrated in Figure 4.

3.3. Identifying Magnetometer Distortion

To prevent heading misalignment, it is necessary to reprocess the magnetometer measurements. By comparing the measured magnetic vector with the local geomagnetic vector, it is possible to identify if the geomagnetic vector has been disturbed. If the magnitude of the measured magnetic field significantly deviates from the expected value, the magnetometer data is considered unreliable.

In this paper, a dynamic switching of the gain factor in the heading channel is implemented by introducing a gain factor, which adapts to different interference environments. The switching logic is initially based on the magnetic field strength level determined from the scalar dynamic magnetometer measurement $m(k)$, defined in Equation (22), where m is the local reference magnetic field value. The weight of the magnetometer in the calculation process is further determined by the difference $\Delta \psi$ between the heading angle derived from the magnetometer ${\psi }_m$ and the heading angle obtained from the gyroscope ${\psi }_g$, defined in Equation (23).

$$m(k)=\left|\sqrt{m_x{(k)}^2+m_y{(k)}^2+m_z{(k)}^2}-m\right|$$

(23)

$$\Delta \psi =\left|{\psi }_g-{\psi }_m\right|$$

(24)

The thresholds for $m(k)$ and $\Delta \psi$ are experimentally determined based on the characteristics of the gyroscope and magnetometer as well as application requirements. The logic for switching the filter gain includes the following scenarios:

Non-magnetic interference mode: $m(k)<0.060m$, $0.1°<\Delta \psi <1°$. In this mode, the magnetometer provides a reliable heading estimate, and the magnetometer is given most of the weight in the calculation.

Weak magnetic interference mode: $m(k)<0.060m$, $0.1°<\Delta \psi <1°$ or $0.060m<m(k)<0.5m$, $\Delta \psi <1°$. In this mode, the uncertainty of the orientation estimated by the accelerometer must be considered, and the gyroscope is given a higher weight in the calculation.

Strong magnetic interference mode: $1°<\Delta \psi$ or $m(k)>0.5m$. In this mode, the system is in a high interference state, and the heading estimate based on the magnetometer is not accurate. Therefore, the cutoff frequency is set to 0, effectively turning off the magnetometer feedback and relying entirely on the gyroscope.

In the presence of geomagnetic vector distortion, only the gravity vector should be used as an observation vector. Once the system is free from this interference, both magnetic and gravity vectors can be used for observation. Under different conditions, the gradient ∇f is given by Equation (24):

$$\nabla f\{\begin{array}{l}{J}_g^T{f}_g，Stronginterferencemode\\ J_{g,b}^T{f}_{g,b}，Weakinterferencemode\end{array}$$

(25)

The gain scheduling for the yaw angle obtained from the magnetometer is illustrated in Figure 5.

4. Experimental Validation and Analysis

Authors should discuss the results and how they can be interpreted from the perspective of previous studies and of the working hypotheses. The findings and their implications should be discussed in the broadest context possible. Future research directions may also be highlighted.

4.1. Experimental Design

To ensure the comprehensiveness of the orientation data calculated in this paper, the following experimental scenarios were designed. The test experiments in this study include the following scenarios: (1) non-interference scenario, (2) application of acceleration interference to the vehicle during the test, (3) application of magnetic interference to the vehicle during the test, and (4) simultaneous application of acceleration and magnetic interference to the vehicle during the test.

The AHRS used in the experiments includes three accelerometers, three gyroscopes, and three magnetometers, with digital interface output. This inertial measurement unit provides measurements of the vehicle’s triaxial linear acceleration, angular velocity, and Earth’s magnetic field. Table 1 provides a description of the sensor parameters in the AHRS. The AHRS prototype is shown in Figure 6.

4.2. Experimental Results

The proposed switching architecture was tested. The results of the gain-scheduled gradient descent method were compared with those of the traditional gradient descent method, as shown in Figure 7, Figure 8 and Figure 9.

In Figure 7, Figure 8 and Figure 9, the comparison between the two algorithms demonstrates that the traditional gradient descent method is prone to sudden changes in the calculated orientation angles due to external interference. In contrast, the algorithm proposed in this paper maintains stable orientation outputs even when subjected to external disturbances.

To validate the effectiveness of the orientation estimation, the results were calculated using the root mean square error (RMSE) method, as shown in Table 2. It is evident that the improved gradient descent method yields significantly better orientation calculation results compared to the traditional gradient descent method, with a substantial increase in accuracy.

5. Conclusions

This paper describes an orientation and heading reference system (AHRS) algorithm with strong anti-interference capabilities. The proposed gain-scheduled gradient descent method employs a switching architecture based on acceleration and magnetic field strength to determine the gain coefficients in the gradient descent algorithm. The proposed algorithm demonstrates excellent damping characteristics against drift errors from gyroscope measurements and noise and interference from accelerometer and magnetometer measurements.

In an interference environment, the roll angle error, pitch angle error, and heading angle error of the proposed method, based on the RMSE method, are 0.0350°, 0.0277°, and 0.9611° respectively. This indicates that the gain-scheduled gradient descent algorithm can achieve stable AHRS output.