Attitude Estimation Using an Enhanced Error-State Kalman Filter with Multi-Sensor Fusion

1  Introduction

In recent years, attitude estimation technology based on Micro-Electro-Mechanical Systems (MEMS) Inertial Measurement Units (IMUs) has become an indispensable core sensing method in fields such as robotic motion control, Unmanned Aerial Vehicle (UAV) flight stabilization, virtual/augmented reality interaction, and human motion capture [1]. However, inherent limitations of MEMS sensors, such as thermal drift and noise, significantly restrict the accuracy and long-term reliability of attitude estimation, while external disturbances and motion-induced acceleration further exacerbate these issues [2]. To overcome these limitations and achieve high-precision, highly robust attitude estimation, it is essential to effectively fuse multi-source sensor data and perform systematic error compensation [3]. Current mainstream attitude estimation methods are mainly divided into two categories: complementary filtering and Kalman filtering [4].

Complementary filtering has a simple structure and low computational overhead, but its estimation accuracy deteriorates sharply in environments with intense motion and magnetic interference. To address these issues, Literature [5] employed a PI controller for sensor fusion, whose integral term estimates gyroscope bias online to suppress attitude drift, with stability guaranteed by Lyapunov theory. To optimize efficiency, Literature [6] developed a gradient descent orientation filter that minimizes the error between calculated and measured vector directions, enabling high-precision updates with minimal computation. Literature [7] presents an adaptive complementary filter that employs least squares estimation to auto-tune its gains, eliminating manual tuning and demonstrating superior performance over fixed-gain filters. Literature [8] proposes a Double-Complementary Filter (DCF) that decouples magnetometer interference from pitch/roll estimation by isolating it to yaw correction within a two-layer architecture, effectively mitigating magnetic disturbances.

Kalman filtering excels in theoretical rigor and accuracy in dynamic environments but heavily relies on precise system models, involves complex parameter tuning processes, and has high computational complexity. To tackle these problems, Literature [9] proposed a novel dual-stage Kalman filter with reduced computational complexity, enabling high-precision, real-time orientation tracking on a low-cost, compact integrated IMU chip. Literature [10] introduced differential manifold theory, describing attitudes in the Lie group space, which better aligns with their geometric nature, thereby resolving the structural distortion issues caused by linear approximation in traditional Kalman filtering in three-dimensional rotation space. Based on the standard Unscented Kalman Filter (UKF) algorithm, Literature [11] enhances the accuracy and robustness of attitude estimation by adaptively adjusting parameters, dynamically processing noise, introducing correction factors, and eliminating anomalous data points. Literature [12] proposes an interval type-3 fuzzy adaptive extended Kalman filter that enhances attitude estimation accuracy for low-cost sensors by dynamically tuning noise covariance.

To enhance the algorithm’s adaptability and robustness in complex dynamic environments, this paper introduces an improved Sage-Husa adaptive mechanism within the ESKF framework. This mechanism dynamically estimates the process noise covariance matrix Q and the measurement noise covariance matrix R online: Q is adjusted adaptively based on the angular rate magnitude, combined with an exponential smoothing strategy to ensure numerical stability; R is updated recursively according to the real-time innovation covariance. A multi-stage anomaly detection mechanism is designed to address the susceptibility of the magnetometer to interference, incorporating dynamic variations in magnetic field intensity, steady-state deviations, and valid range checks. Based on this, a dual-path update strategy for yaw angle correction is implemented: when the yaw angle deviation is significant, a full ESKF measurement update is performed; for minor deviations, a direct correction method based on the minimum-bias rotation quaternion is adopted, ensuring accuracy while significantly reducing computational complexity.

2  Methodology

The ESKF algorithm directly estimates the error quantities of the system state and performs linearization operations in the error state space. This approach effectively avoids direct linearization of the original complex nonlinear system equations, thereby reducing approximation errors introduced by linearization and lowering computational burden [13,14]. To further enhance the algorithm’s adaptability and robustness under complex dynamic working conditions, this paper introduces key enhancements to the ESKF framework: a Sage-Husa adaptive mechanism is incorporated to respond in real time to system state variations and environmental disturbances, enabling dynamic online updates of the process and measurement noise covariance matrices. To reduce the computational complexity of the algorithm, a magnetic heading correction method based on the minimum-bias rotation quaternion is proposed. When the deviation angle is below a set threshold, this method replaces the magnetometer update module in the ESKF, ensuring both accuracy in yaw angle correction and reduced computational load. The overall architecture of the algorithm is shown in Fig. 1.

Figure 1: Framework diagram for improving ESKF algorithm

2.1 Attitude Calculation Based on Quaternions

To describe the attitude transformation in three-dimensional space, it is necessary to establish a spatial attitude transformation model between the body coordinate system (b-frame) and the navigation coordinate system (n-frame, using the NED (North-East-Down) coordinate system). Due to the gimbal lock problem associated with Euler angles, unit quaternions are employed for attitude modeling and updating:

q=[q0q1q2q3]T(1)

The dynamic change of attitude is driven by the angular velocity ωb=[ωx,ωy,ωz]T measured by the gyroscope. The continuous-time differential equation for the quaternion is:

q˙=12q⊗[0ωb]=12Ω(ωb)q(2)

where ⊗ represents quaternion multiplication, and Ω(ωb) is the skew-symmetric matrix of the angular rate. The differential equation is discretized using the first-order Runge-Kutta method (Runge-Kutta 1). Given the sampling period Δt, the quaternion update formula is:

qk=qk−1+12Ω(ωkb)qk−1Δt(3)

The vector transformation from the body frame to the navigation frame can be achieved using the direction cosine matrix:

Rnb=[1−2(q22+q32)2(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)1−2(q12+q32)2(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)1−2(q12+q22)](4)

Using the Z-Y-X Euler angle rotation sequence, the attitude angles (Roll-ϕ, Pitch-θ, Yaw-ψ) can be derived as:

ϕ=arctan⁡2(2(q0q1+q2q3)1−2(q12+q22))(5)

θ=arcsin⁡(2(q0q2−q3q1))(6)

ψ=arctan⁡2(2(q0q3+q1q2)1−2(q22+q32))(7)

2.2 Attitude Estimation Based on ESKF

The ESKF decomposes the system state into a nominal state and an error state. The nominal state is typically predicted directly through the integration of IMU or other sensor data. This nonlinear integration process ignores various sources of error. As a result, the nominal state can be computed rapidly but exhibits low accuracy and tends to diverge over time. The error state represents the difference between the nominal state and the true state. Instead of directly estimating the complete system state, the ESKF focuses on estimating this error state [15]. In the algorithm presented in this paper, the nominal state of the system is defined as the attitude quaternion q, which is predicted directly by integrating the gyroscope’s angular velocity. The error state is defined as a six-dimensional vector δx:

δx=[δα1×3,δβ1×3]T(8)

where δα1×3 represents the attitude error angle and δβ1×3 denotes the gyroscope bias error. The compensated gyroscope measurement is obtained by subtracting the bias:

ωcorr=ω−δβ1×3(9)

The compensated gyroscope measurement ωcorr is used to propagate the predicted attitude value via the first-order Runge-Kutta method (Formula (3)). The error state of the system is updated by the following discrete system model:

{F=I6+[03×3−Rbn03×303×3]Δtδxk|k−1=Fδxk−1|k−1G=I6Pk|k−1=FPk−1|k−1FT+GQkGT(10)

where F is the state transition matrix, G is the noise coupling matrix, P is the error state covariance matrix, and Q is the process noise covariance matrix. The acceleration data is utilized for measurement update, and the acceleration observation residual Za is constructed as follows:

{gnorm=g‖g‖g^n=q⊗−gnorm⊗q−1gn=[0;0;−1]Za=g^n−gn(11)

where g^n denotes the gravitational acceleration vector in the navigation frame (n-frame), obtained by rotating the normalized accelerometer measurements from the body frame (b-frame) using the currently estimated attitude quaternion q, and gn is the theoretical gravity direction in the navigation frame. Due to the uncertainty in the Z-axis observation, only the X and Y directional observation information is used to construct a 2 × 6 measurement matrix Ha:

Ha=[0−11000000000](12)

Using the measurement matrix Ha and the observation noise covariance matrix Ra, the Kalman gain Ka, the error state covariance matrix P, innovation vector va, and error state vector δx updated:

{Ka=Pk|k−1HaT(HaPk|k−1HaT+Ra)−1Pk|k=(I−KaHa)Pk|k−1(I−KaHa)T+KaRaKaTva=Za(1:2)−Haδxk|k−1δxk|k=δxk|k−1+Kava(13)

Finally, the attitude error δα1×3 is converted into a small-angle quaternion to correct the predicted attitude value, and the error state is reset to zero to prevent the propagation of error accumulation:

{qk=δq(δα1×3)⊗qk|k−1δα1×3=0(14)

The magnetometer measurement update is a key step in the attitude estimation filter for correcting heading angle drift. The input raw magnetometer vector m is normalized to obtain a unit vector mnorm. which is subsequently transformed into the navigation frame (n-frame) using the current attitude quaternion q, yielding the predicted magnetic field vector h the navigation frame:

{mnorm=m‖m‖h=q⊗mnorm⊗q−1(15)

To eliminate the influence of magnetic inclination and provide only heading (yaw) observation, the predicted vector his projected onto the local horizontal plane to construct a reference vector b. Subtracting b from h the magnetometer observation residual Zm on the horizontal plane:

{b=[‖[h(1),h(2)]‖,0,h(3)]TZm=h−b(16)

The magnetometer measurement matrix Hm is constructed based on the skew-symmetric matrix of the reference vector b:

{⌊b⌋=[0−bzbybz0−bx−bybx0]Hm=[⌊b⌋,03×3](17)

Similar to the accelerometer measurement update, the Kalman gain Km, error state covariance matrix P, innovation vector vm, and error state vector δx are updated using the measurement matrix Hm and the magnetometer observation noise covariance matrix Rm:

{Km=Pk|k−1HmT(HmPk|k−1HmT+Rm)−1Pk|k=(I−KmHm)Pk|k−1(I−KmHm)T+KmRmKmTvm=Zm−Hmδxk|k−1δxk|k=δxk|k−1+Kmvm(18)

Before feedback correction, the roll and pitch error components in the error state vector are set to zero, retaining only the heading angle error term. The attitude error δα1×3 is converted into a small-angle quaternion to correct the predicted attitude value, and the error state is reset to zero to prevent the propagation of error accumulation:

{δα(1:2)=02×1qk=δq(δα1×3)⊗qk|k−1δα1×3=0(19)

2.3 Adaptive ESKF Based on Improved Sage-Husa

The aforementioned ESKF algorithm assumes that the statistical characteristics of the process noise and observation noise are known and time-invariant. However, in practical applications, these characteristics are often unknown or time-varying, leading to performance degradation of filters based on fixed noise parameters [16]. The Sage-Husa adaptive filtering method can utilize real-time measurement data to adaptively adjust the process noise covariance matrix Q and the observation noise covariance matrix R, effectively mitigating model mismatch and improving filtering accuracy. The adaptive correction formulas for R and Q in Sage-Husa are as follows:

{dk=1−b1−bk+1Rk=(1−dk−1)Rk−1+dk−1(vvT−HPk|k−1HT)Qk=dk−1[KvvTKT+Pk|k−FPk−1|k−1FT]+(1−dk−1)Qk−1(20)

However, when both the process noise covariance matrix Q and the observation noise covariance matrix R are unknown, the Sage-Husa algorithm cannot estimate them simultaneously [17]. In practical applications, the process noise covariance matrix is associated with system characteristics and is less prone to change, while the observation noise covariance matrix is susceptible to external influences. Therefore, this paper adaptively adjusts the process noise covariance matrix Q using exponential smoothing updates based on the magnitude gyronorm of the gyroscope angular velocity, while simultaneously employing the Sage-Husa algorithm to adaptively update the observation noise covariance matrix R. The specific update rules are defined as follows:

{λtarget=λmin+(λmax−λmin)(1−exp⁡(gyronormgyrosensitivity))λk=(1−β)λk−1+β⋅λtarg⁡etQk=[λkI3×30003×3]dk=1−b1−bk+1Rk=(1−dk−1)Rk−1+dk−1(vvT−HPk|k−1HT)(21)

In the ESKF algorithm, introducing an improved Sage-Husa adaptive filtering algorithm enables dynamic adjustment of the measurement noise covariance matrix, thereby better handling sensor noise variations and abnormal interference.

2.4 Magnetic Heading Correction Using a Minimum-Bias Rotation Quaternion

This section proposes a magnetic heading correction method based on the minimum deviation rotation quaternion to reduce the computational complexity of the yaw angle correction process. The measured value of the magnetometer in the b-frame be:

Mb=[Mxb,Myb,Mzb](22)

Under ideal undisturbed conditions, when the navigation coordinate system (n-frame) aligns with the body coordinate system (b-frame), the magnetic field should primarily distribute along the horizontal and vertical directions [18]:

Mn=[MN,0,MD]T(23)

where MN represents the north-component of the horizontal magnetic field, and MD denotes the vertical component. The relationship between Mb and Mn satisfies:

[MxbMybMzb]=[MNcos⁡θcos⁡ψ−MDsin⁡θMN(sin⁡ϕsin⁡θcos⁡ψ−cos⁡ϕsin⁡ψ)+MDsin⁡ϕcos⁡θMN(cos⁡ϕsin⁡θcos⁡ψ+sin⁡ϕsin⁡ψ)+MDcos⁡ϕcos⁡θ](24)

Thus, the estimated yaw angle calculated from the magnetometer can be obtained as:

ψm=arctan⁡(Mzbsin⁡ϕ−Mybcos⁡ϕMxbcos⁡θ+Mybsin⁡θsin⁡ϕ+Mzbsin⁡θcos⁡ϕ)(25)

Define the deviation between the magnetometer-estimated heading value and the ESKF-calculated value Δψ as:

Δψ=ψm−ψ(26)

This paper adopts a dual-mode adaptive yaw angle correction mechanism. By setting a threshold for the absolute deviation between the magnetometer-estimated yaw angle and the ESKF-fused output yaw angle, dynamic switching of fusion strategies is achieved, thereby balancing accuracy and computational efficiency. The specific strategies are as follows:

When the absolute deviation |Δψ| exceeds the preset threshold, the complete ESKF measurement update process is activated. This mode fully utilizes the global observation information from the magnetometer to precisely correct the heading error, ensuring high convergence accuracy and stability even in the presence of significant deviations.

When the absolute deviation is less than or equal to the threshold, a computationally efficient direct quaternion compensation method is employed for fine-tuning. This approach directly adjusts the current attitude through a small-angle rotation quaternion around the Z-axis, significantly reducing computational complexity while enabling rapid and precise adjustments for minor deviations. A small-angle rotation correction quaternion Δq around the Z-axis is constructed, and the attitude is updated using quaternion multiplication to transform it into the main state:

{θ=kmag⋅ΔψΔq=[cos(θ/2),0,0,sin(θ/2)]Tqnew=Δq⊗q(27)

where kmag is the magnetometer correction coefficient, which controls the correction rate and stability.

2.5 Anti-Interference Protection Strategy

2.5.1 Hard Iron and Soft Iron Compensation

In attitude estimation systems, magnetometer measurements are often affected by environmental magnetic fields and the structure of the carrier, primarily manifesting as hard iron effects (fixed biases) and soft iron effects (distortions in scale and orientation across axes) [19]. Without proper compensation, the magnetometer output exhibits an ellipsoidal distribution rather than the ideal spherical distribution centered at the origin with a radius equal to the Earth’s magnetic field strength, significantly degrading attitude estimation accuracy. The measurement model of the magnetometer can be expressed as:

mraw=Smture+b+n(28)

where mraw represents the raw three-axis magnetometer measurement vector at a given time, S is the soft iron effect matrix, b is the hard iron bias, and n denotes noise. Under ideal conditions, the calibrated magnetometer output should lie on a spherical surface across different attitudes. However, in the presence of interference, the distribution of deviates from a sphere, forming an ellipsoid.

To achieve calibration, an ellipsoid fitting method is employed. The core idea is to fit the magnetometer data points to a three-dimensional ellipsoid model and then decompose the parameters to compensate for hard and soft iron effects. The general quadratic form of the ellipsoid equation is:

mrawTAmraw+dTmraw+e=0(29)

where mraw is the original measurement value of the magnetometer, and A is the symmetric positive definite matrix. The calibration process of a magnetometer requires moving the device along an infinite shape (∞) path to obtain sufficient magnetic field data in various poses and directions. Expand the ellipsoid equation into algebraic form:

Ax2+By2+Cz2+2Dxy+2Exz+2Fyz+2Gx+2Hy+2Iz+J=0(30)

Define parameter vector p=[A,B,C,D,E,F,G,H,I,J]T, Construct design vectors for each measurement point:

Vi=[xi2,yi2,zi2,2xiyi,2xizi,2yizi,2xi,2yi,2zi,1](31)

Constructing Design Matrix M=[V1T;V2T;⋯;VNT] to Solve Constrained Optimization Problems:

min‖p‖=1‖Mp‖2(32)

The solution to this problem is the eigenvector corresponding to the minimum eigenvalue of the matrix MT M. Reconstruct the symmetric matrix and vector from the solution p:

{A=[ADEDBFEFC]d=2[GHI]e=J(33)

Hard iron bias is given by the center of the ellipsoid:

b=−12A−1d(34)

To compensate for soft iron effects, the fitted ellipsoid is normalized. From the ellipsoid equation, it follows that:

W=AbTAb−e(35)

where W is the normalized ellipsoid shape matrix, reflecting the scaling and rotational characteristics of the soft iron effect in three-dimensional space W meets:

(mraw−b)TW(mraw−b)=1(36)

Performing eigenvalue decomposition on W:

W=UΛUT(37)

where U is an orthogonal matrix and Λ is a diagonal matrix. The calibration matrix is derived as:

C=UΛ12UT(38)

The calibrated magnetometer output is then given by:

mcal=C(mraw−b)(39)

Theoretically, the calibrated data should satisfy all points lie on a spherical surface, thereby eliminating the impact of hard and soft iron effects on attitude estimation.

2.5.2 Linear Acceleration and Magnetic Interference

Linear acceleration disrupts the accuracy of the gravitational component in accelerometer measurements, which is a major source of attitude estimation errors [20]. The proposed algorithm uses the Exponentially Weighted Moving Average (EWMA) to compute a smoothed value anormEWMA tracking the long-term trend of gravitational acceleration while filtering out transient noise:

anormEWMA=(1−αa)anormEWMA+αaanorm(40)

where αa is the smoothing factor. Abnormalities are detected by comparing the absolute deviation between the current magnitude and the EWMA baseline against a preset threshold. If the threshold is exceeded, significant linear acceleration interference is deemed present, and accelerometer-based observation updates are suspended. To avoid estimation jitter caused by immediate resumption after transient interference, a delayed recovery mechanism is implemented: accelerometer updates are only re-enabled after N consecutive cycles of normal data.

The Earth’s magnetic field is susceptible to disturbances from ferromagnetic materials in the environment, causing drift in yaw angle estimation [21]. Similar to acceleration detection, the system calculates the relative EWMA deviation δmrel of the magnetic field magnitude for anomaly detection:

δmrel=|mnorm−mnormEWMA|mnormEWMA(41)

If δmrel exceeds a threshold, the current magnetic environment is considered unreliable, and magnetometer-based measurement updates are immediately suspended. Magnetometer updates for yaw angle are only re-enabled after N consecutive cycles of normal data.

The anomaly detection module not only relies on relative EWMA deviation to perceive signal mutations, but also sets an absolute threshold as a safety guarantee to ensure that the system can effectively identify and respond to continuous external interference.

3  Experimental Tests and Results Analysis

To evaluate the performance of the improved ESKF attitude estimation method (denoted as ESKF-MYC) under different operational conditions, this study conducted two categories of validation experiments: dynamic and static mixed experiment and magnetic interference experiments.

The dynamic and static mixed experiment utilized the real-time attitude monitoring device shown in Fig. 2 as the data acquisition platform. This device collects raw data from MEMS inertial sensors and transmits it in real time to the monitoring terminal (Fig. 3).

Figure 2: Real time attitude monitoring equipment

Figure 3: Monitoring terminal interface

To prevent limitations that could arise from a single hardware configuration and to further examine the algorithm’s generalization capability and resilience to magnetic interference across IMUs with varying noise characteristics, this study also selected sequences involving magnetic interference from the public Berlin Robust Orientation Estimation Assessment Dataset (BROAD) [22] for additional validation.

To ensure objective comparison, all collected data—whether from our self-developed platform or the public dataset—were processed offline in MATLAB under a unified evaluation framework. In terms of comparative algorithm selection, in addition to the Error State Kalman Filter (ESKF) algorithm, two representative complementary filter-based attitude estimation methods were introduced: the Mahony algorithm [5] and its improved version, the Madgwick algorithm [6]. The Madgwick algorithm enhances the attitude estimation process by introducing a gradient descent strategy, which effectively improves attitude convergence accuracy and stability while reducing the number of tuning parameters to one. The two sets of experiments conducted a comparative analysis of the performance of the aforementioned algorithms and the proposed ESKF-MYC algorithm, and the results are discussed.

To achieve high-precision, low-power attitude measurement, the device integrates two MEMS sensors: the LSM6DSOW and the LIS2MDL. The sensor parameters are listed in Table 1, where A denotes the accelerometer and G denotes the gyroscope.

3.1 Dynamic and Static Mixed Experiment

To validate the performance of the proposed attitude estimation method in mixed static-dynamic scenarios, the following experimental procedure was designed: The attitude monitoring device was fixed on one side of a rigid acrylic board, while a high-precision attitude sensor HWT905 (accuracy: ±0.05°) was installed on the opposite side as the ground truth reference (REF.). The experiment was conducted in an open indoor area with a uniform magnetic field distribution and no external interference. Prior to the experiment, the attitude monitoring node was calibrated to align its initial attitude with the reference sensor. During the experiment, complex high-dynamic attitude changes were simulated by rapidly and irregularly moving the acrylic board. Each movement was followed by a period of static holding, and this sequence was repeated four times. In the final stage of the experiment, the movement speed was further increased to test the algorithm’s adaptability and convergence capability during rapid transitions between dynamic and static conditions. Data from both the attitude monitoring node and the reference sensor were synchronously collected and uploaded to the host computer, with a data reporting period set to 3.5 ms. The data acquired by the attitude monitoring node was imported into MATLAB for offline processing, where the attitude angles were computed and compared with the reference attitude angles for analysis. The parameter settings for the comparative algorithms are as follows: Mahony: KP = 10, KI = 0.02; Madgwick: β = 0.8; ESKF: Q = diag([1e−4, 1e−4, 1e−4, 0, 0, 0]), Ra = 10 I2, Rm = 5 I3, The proposed ESKF-MYC was initialized with the same parameters as the ESKF, with the magnetic anomaly correction gain kmag set to 0.002 and the mode-switching threshold set to 0.15 rad.

As shown in the static-dynamic solution results of the roll angle (ROLL) in Figs. 4 and 5, during the low-speed motion phase from 0 to 200 s, all algorithms are able to track the reference attitude angle well, with the solution results aligning closely with the ground truth. However, when the experimental setup abruptly enters a rapid motion state at 213 s, significant differences emerge in the responses of the algorithms. Among them, the Mahony algorithm responds most quickly to the dynamic change, but its solution exhibits noticeable jitter, indicating insufficient stability. The ESKF-MYC algorithm follows closely, responding rapidly while maintaining a smooth output trajectory, demonstrating good dynamic performance. In contrast, the ESKF and Madgwick algorithms show relatively slower responses to the rapid motion, with a certain degree of lag observed.

Figure 4: Calculation results of roll angle in dynamic and static mixed experiment

Figure 5: Enlarged detail of #1 location

Table 2 presents the roll angle estimation errors of each algorithm in dynamic and static mixed experiment, the ESKF-MYC algorithm achieved the lowest Root Mean Square Error (RMSE = 5.638°), indicating superior overall estimation accuracy and strong robustness against outliers. The ESKF algorithm recorded the lowest Mean Absolute Error (MAE = 1.499°), suggesting more stable estimation performance under most conditions. The Mahony algorithm, while outperforming the Madgwick algorithm (MAE = 3.640°) with an MAE of 2.942°, exhibited a significantly higher RMSE of 8.069°, reflecting noticeable jitter or deviation during dynamic phases. The Madgwick algorithm yielded the highest RMSE (11.467°), indicating poor adaptability to abrupt changes in motion states. Overall, the ESKF-based algorithms demonstrated better performance in mixed static-dynamic scenarios, with ESKF-MYC showing notable effectiveness in controlling maximum error, while the standard ESKF slightly outperformed in terms of steady-state accuracy.

As shown in Figs. 6 and 7, the pitch angle solution results are similar to those of the roll angle. When confronted with sudden angular changes, the Mahony algorithm demonstrates the fastest response and tracks the reference angle effectively. The ESKF-MYC algorithm follows with a slightly slower response speed, while both the Madgwick and ESKF algorithms exhibit relatively slower responses along with significant estimation deviations.

Figure 6: Calculation results of pitch angle in dynamic and static mixed experiment

Figure 7: Enlarged detail of #2 location

Table 3 presents the pitch angle estimation errors of each algorithm in dynamic and static mixed experiment. The ESKF-MYC algorithm achieves the best performance on both error metrics, with an RMSE of 2.616° and an Mean Absolute Error (MAE) of 0.920°, demonstrating the highest overall accuracy. The ESKF algorithm ranks second with an MAE of 1.017°. Although the Mahony algorithm has an MAE of 1.268°, its RMSE (3.107°) is superior to that of the Madgwick algorithm (4.972°), indicating better stability. The Madgwick algorithm performs relatively weakly on both metrics. The ESKF-MYC algorithm not only maintains high precision (lowest MAE) but also exhibits the best robustness to outliers (lowest RMSE), confirming its stable performance across various motion states. While the ESKF algorithm has a low MAE, its relatively higher RMSE suggests the occurrence of larger errors during rapid motion. The Mahony algorithm delivers balanced performance with mid-range results for both RMSE and MAE. The notably higher RMSE of the Madgwick algorithm indicates its greater sensitivity to changes in motion state.

As depicted in Figs. 8 and 9, the yaw angle estimation results demonstrate that the ESKF-MYC algorithm achieves the fastest response to angular changes, producing smooth trajectories that align closely with the reference values. The ESKF algorithm follows with a slightly slower response yet maintains good tracking accuracy. In contrast, the Mahony algorithm exhibits significant jitter in its output, while the Madgwick algorithm responds sluggishly and fails to reflect the actual attitude variations effectively.

Figure 8: Calculation results of yaw angle in dynamic and static mixed experiment

Figure 9: Enlarged detail of #3 location

Table 4 presents the yaw angle estimation errors of each algorithm in dynamic and static mixed experiment. The results indicate that the ESKF-MYC algorithm delivers the best performance in terms of Root Mean Square Error (RMSE = 6.315°), demonstrating superior overall estimation accuracy and stability. The ESKF algorithm follows closely (RMSE = 8.831°), and slightly outperforms the ESKF-MYC algorithm in Mean Absolute Error (MAE = 2.569° vs. 2.762°), indicating exceptionally high precision under most normal conditions. In contrast, both the Mahony algorithm (RMSE = 9.418°, MAE = 4.028°) and the Madgwick algorithm (RMSE = 14.068°, MAE = 4.504°) exhibit significantly larger errors.

In summary, the ESKF-MYC algorithm achieves the lowest RMSE in both the roll (RMSE: 5.638°) and yaw (RMSE: 6.315°) dimensions, and ranks first in the pitch angle (RMSE: 2.616°) as well. This fully demonstrates the effectiveness of its improvement strategy, particularly in mixed static-dynamic scenarios that require controlling maximum error and ensuring overall stability, where ESKF-MYC exhibits the strongest comprehensive robustness and the highest accuracy. The ESKF algorithm achieves the best MAE indicators in both the pitch (MAE: 1.017°) and yaw (MAE: 2.569°) angles, indicating that its fundamental design can provide extremely precise estimations under normal conditions without abnormal interference. Its slightly inferior performance in roll angle estimation compared to ESKF-MYC reveals the enhancement effect of the improved algorithm in handling specific dynamic errors. The Mahony and Madgwick algorithms generally exhibit higher errors in three-axis estimation compared to the ESKF-based algorithms, especially in the roll and yaw angles where dynamic performance requirements are higher. This indicates that in scenarios involving rapid motion, the ESKF framework based on the error-state Kalman filter holds a clear advantage over complementary filter methods in terms of both accuracy and stability.

3.2 Magnetic Interference Experiment

The BROAD dataset utilizes a high-precision optical motion capture (OMC) system to obtain the ground-truth attitude of the measurement unit, providing a high-accuracy attitude reference in interference-prone environments. This enables a comprehensive, fair, and reproducible evaluation of algorithm performance. The dataset employs a myon aktos-t nine-axis inertial measurement unit (IMU) to collect IMU data, with a gyroscope range of ±2000°/s, an accelerometer range of ±16 g, and a magnetometer range of ±1 mT. The reference attitude ground truth is provided by an OptiTrack optical motion capture system at a sampling rate of 120 Hz. A specially designed rigid structure ensures a fixed relative pose between the IMU sensor and the optical markers, while an optimization-based time synchronization method aligns timestamps across systems, effectively reducing spatiotemporal deviations between heterogeneous data sources and achieving a comprehensive sampling rate of 287 Hz. To validate the anti-interference performance of the algorithms in magnetically disturbed environments, this section selects data containing magnetic interference scenarios for analysis. The parameter settings for the comparative algorithms are as follows:

Mahony: KP = 5, KI = 0.005; Madgwick: β = 0.5 ;ESKF: Q = diag([5e−5, 5e−5, 5e−5, 0, 0, 0]), Ra = 6.5 I2, Rm = 10 I3. The proposed ESKF-MYC was initialized with the same parameters as the ESKF, with the magnetic anomaly correction gain kmag set to 0.002 and the mode-switching threshold set to 0.3 rad.

Fig. 10 shows the variation curve of magnetic field intensity over a period of 160 s, clearly illustrating the movement of the IMU unit in the vicinity of the magnetic interference source.

Figure 10: Variation curve of magnetic field intensity

As shown in Figs. 11 and 12, the overall impact of magnetic interference on the roll angle solution is limited, but the algorithms exhibit significant differences in dynamic performance. The ESKF-MYC, ESKF, and Madgwick algorithms are all able to accurately track rapid attitude changes, whereas the Mahony algorithm demonstrates noticeable overshooting and jitter.

Figure 11: Calculation results of roll angle in magnetic interference experiment

Figure 12: Enlarged detail of #4 location

As can be seen from the detailed roll angle (ROLL) solution results in Table 5, the estimation accuracy of the algorithms shows a clear gradient difference. Specifically: the ESKF-MYC algorithm performs optimally, achieving a root mean square error (RMSE) of 4.229° and a mean absolute error (MAE) of 2.326°; the ESKF algorithm follows with corresponding errors of 4.777° and 2.583°; the Madgwick algorithm ranks third with errors of 9.002° and 4.745°; while the Mahony algorithm has the largest solution errors, with RMSE and MAE as high as 16.874° and 9.006°, respectively, indicating significantly lower accuracy compared to the other algorithms. Compared to the baseline ESKF algorithm, the ESKF-MYC algorithm further reduces the RMSE by approximately 11.5%.

As shown in Figs. 13 and 14, the overall impact of magnetic interference on pitch angle estimation remains relatively limited, with all algorithms generally exhibiting smaller deviations in this dimension compared to other axes. However, notable differences emerge in dynamic performance: both the ESKF-MYC and ESKF algorithms demonstrate accurate tracking capability during rapid attitude variations, while the Madgwick and Mahony algorithms exhibit more pronounced overshooting and jitter, indicating their relatively inferior dynamic response performance and stability.

Figure 13: Calculation results of pitch angle in magnetic interference experiment

Figure 14: Enlarged detail of #5 location

The pitch angle solution results in Table 6 clearly demonstrate the accuracy gradient among the algorithms. The ESKF-MYC algorithm achieves optimal performance with errors of 2.395° (RMSE) and 1.745° (MAE), representing an approximate 10.8% improvement in accuracy compared to the ESKF algorithm, which has an RMSE of 2.686° and MAE of 1.849°. The errors of the Madgwick algorithm (RMSE: 4.327°, MAE: 2.849°) and Mahony algorithm (RMSE: 5.471°, MAE: 3.812°) increase progressively, ranking third and fourth respectively in terms of performance.

As shown in Figs. 15 and 16, magnetic interference has a significant impact on the yaw angle solution. At position #6, the ESKF-MYC algorithm detects the magnetic interference and disables the magnetometer correction, thereby effectively immunizing itself against this disturbance. The other algorithms, however, are all affected by the sudden magnetic field change to varying degrees, with the Madgwick algorithm experiencing the most severe impact. At position #7, both the ESKF-MYC and ESKF algorithms are able to track the reference angle well, while the Mahony and Madgwick algorithms still exhibit noticeable estimation deviations.

Figure 15: Calculation results of yaw angle in magnetic interference experiment

Figure 16: Enlarged detail of #6 and #7 location. (a) Enlarged detail of #6 location; (b) Enlarged detail of #7 location

As evidenced by the yaw angle estimation errors in Table 7, significant performance disparities exist among the algorithms in magnetically disturbed environments. The ESKF-MYC algorithm outperforms all comparative methods in both Root Mean Square Error (RMSE = 6.469°) and Mean Absolute Error (MAE = 4.374°), demonstrating superior anti-interference capability and estimation accuracy. The ESKF algorithm follows with RMSE and MAE of 8.441° and 5.220°, respectively, maintaining stable overall performance. The Madgwick algorithm shows substantially increased errors (RMSE = 11.914°, MAE = 7.037°), indicating relatively weak robustness against magnetic disturbances. The Mahony algorithm exhibits the largest estimation errors (RMSE = 20.523°, MAE = 11.843°), significantly exceeding other algorithms and revealing poor adaptability in strong magnetic interference environments. In summary, the ESKF-MYC algorithm demonstrates the strongest robustness and highest precision in yaw angle estimation, making it particularly suitable for practical applications involving magnetic disturbances.

The experimental results in this section demonstrate that the ESKF-MYC algorithm achieves the best overall performance. It delivers the highest accuracy in both roll (ROLL) and yaw (YAW) angles, while maintaining near-optimal performance in pitch (PITCH) angle estimation, exhibiting excellent anti-interference capability and dynamic stability. The ESKF algorithm shows robust performance, ranking second. In contrast, both the Madgwick and Mahony algorithms, particularly in yaw angle estimation and anti-magnetic interference performance, demonstrate significantly larger errors compared to the ESKF-based algorithms. Overall, the ESKF-MYC algorithm demonstrates substantial advantages in both accuracy and robustness under complex scenarios, making it more suitable for practical applications involving magnetic disturbances and rapid motions.

4  Algorithm Complexity Analysis

This paper proposes a dual-mode correction strategy for yaw angle estimation: a computationally efficient quaternion-based direct correction method is employed for small-angle errors, while the system switches to a higher-precision adaptive ESKF algorithm for large-angle error conditions. The distribution of yaw angle errors from the magnetic interference experiment is shown in the Fig. 17, with the mode-switching threshold set at 0.3 rad. The time and space complexity of the two correction algorithms are compared in the following Table 8:

Figure 17: Variation of yaw angle deviation angle in magnetic interference experiment

All pose estimation algorithms in this study were tested on the MATLAB platform, using a laptop equipped with an AMD Ryzen 7 6800H processor and 16 GB of memory. In the 160-s magnetic interference data experiment, the processing time of the ESKF-MYC algorithm was only 1.111 s, which is approximately 11.5% faster than the 1.255 s required by the standard ESKF algorithm. In the 240-s mixed experiment involving both dynamic and static phases, with the mode switching threshold set to 0.15 rad, the processing time of the proposed algorithm was only 1.410 s, significantly outperforming the 1.671 s of the standard ESKF algorithm, with a computational efficiency improvement of approximately 15.6%. The results demonstrate that the proposed dual-mode switching strategy effectively enhances computational efficiency while maintaining estimation accuracy.

5  Conclusion

This research focuses on enhancing the accuracy of attitude estimation based on inertial measurement units. Through methodological improvements and systematic experimental validation, a multi-sensor fusion attitude estimation method based on an improved Error-State Kalman Filter (ESKF) is proposed. The approach deeply integrates two key innovations within the standard ESKF framework: a real-time measurement noise estimation strategy using an improved Sage-Husa algorithm, and a dual-mode switching method for yaw angle correction. These enhancements significantly improve the algorithm’s adaptability and estimation accuracy in mixed static-dynamic scenarios.

Experimental results under mixed static-dynamic environmental conditions demonstrate that the proposed ESKF-MYC algorithm achieves optimal estimation accuracy in both roll angle (RMSE = 5.638°) and yaw angle (RMSE = 6.315°), while also exhibiting the best performance in pitch angle (RMSE = 2.616°), fully validating the effectiveness of the proposed improvement strategies.

Magnetic interference experiments based on the BROAD public dataset further confirm that the algorithm maintains excellent anti-interference capability and stable dynamic tracking performance even under complex magnetic environmental disturbances, while demonstrating good generalization across different hardware platforms and noise characteristics.

Complexity analysis indicates that the dual-mode correction strategy adopted in this study significantly reduces computational time compared to the standard ESKF algorithm while maintaining yaw angle estimation accuracy, showing potential for application in real-time embedded systems.

Acknowledgement: None.

Funding Statement: This research received no specific grants from public, commercial, or not-for-profit funding agencies.

Author Contributions: Yu Tao was responsible for the primary research execution, including algorithm development, implementation, data analysis, and drafting the manuscript. Yang Jie and Tian Yin contributed to the manuscript by providing revision suggestions. All authors reviewed the results and approved the final version of the manuscript.

Availability of Data and Materials: The authors confirm that the data supporting the findings of this study are available within the article.

Ethics Approval: Not applicable.

Conflicts of Interest: The authors declare no conflicts of interest to report regarding the present study.

References

1. Lan J, Wang K, Song S, Li K, Liu C, He X, et al. Method for measuring non-stationary motion attitude based on MEMS-IMU array data fusion and adaptive filtering. Meas Sci Technol. 2024;35(8):086304. doi:10.1088/1361-6501/ad44c8. [Google Scholar] [CrossRef]

2. Candan B, Soken HE. Robust attitude estimation using magnetic and inertial sensors. IFAC-PapersOnLine. 2023;56(2):4502–7. doi:10.1016/j.ifacol.2023.10.941. [Google Scholar] [CrossRef]

3. Narkhede P, Poddar S, Walambe R, Ghinea G, Kotecha K. Cascaded complementary filter architecture for sensor fusion in attitude estimation. Sensors. 1937 2021;21(6):1937. doi:10.3390/s21061937. [Google Scholar] [PubMed] [CrossRef]

4. Kottath R, Narkhede P, Kumar V, Karar V, Poddar S. Multiple model adaptive complementary filter for attitude estimation. Aerosp Sci Technol. 2017;69(1):574–81. doi:10.1016/j.ast.2017.07.011. [Google Scholar] [CrossRef]

5. Mahony R, Hamel T, Pflimlin JM. Nonlinear complementary filters on the special orthogonal group. IEEE Trans Autom Control. 2008;53(5):1203–18. doi:10.1109/TAC.2008.923738. [Google Scholar] [CrossRef]

6. Madgwick SOH, Harrison AJL, Vaidyanathan R. Estimation of IMU and MARG orientation using a gradient descent algorithm. In: Proceedings of the 2011 IEEE International Conference on Rehabilitation Robotics; 2011 Jun 29–Jul 1; Zurich, Switzerland. doi:10.1109/ICORR.2011.5975346. [Google Scholar] [PubMed] [CrossRef]

7. Narkhede P, Joseph Raj AN, Kumar V, Karar V, Poddar S. Least square estimation-based adaptive complimentary filter for attitude estimation. Trans Inst Meas Control. 2019;41(1):235–45. doi:10.1177/0142331218755234. [Google Scholar] [CrossRef]

8. Lu Y, Wang C, He Q. Double-complementary filter attitude estimator for decoupling magnetometer interference on pitch and roll. J Phys Conf Ser. 2025;3032(1):012019. doi:10.1088/1742-6596/3032/1/012019. [Google Scholar] [CrossRef]

9. Sabatelli S, Galgani M, Fanucci L, Rocchi A. A double-stage Kalman filter for orientation tracking with an integrated processor in 9-D IMU. IEEE Trans Instrum Meas. 2013;62(3):590–8. doi:10.1109/TIM.2012.2218692. [Google Scholar] [CrossRef]

10. Bernal-Polo P, Martínez-Barberá H. Kalman filtering for attitude estimation with quaternions and concepts from manifold theory. Sensors. 2019;19(1):149. doi:10.3390/s19010149. [Google Scholar] [PubMed] [CrossRef]

11. Chen S, Hu F, Chen Z, Wu H. Correction method for UAV pose estimation with dynamic compensation and noise reduction using multi-sensor fusion. IEEE Trans Consum Electron. 2024;70(1):980–9. doi:10.1109/TCE.2023.3339729. [Google Scholar] [CrossRef]

12. Faraji J, Keighobadi J, Janabi-Sharifi F. Design and implementation of an adaptive extended Kalman filter with interval type-3 fuzzy set for an attitude and heading reference system. Signal Process. 2025;233(1):109947. doi:10.1016/j.sigpro.2025.109947. [Google Scholar] [CrossRef]

13. Ren Z, Liu S, Dai J, Lv Y, Fan Y. Research on kinematic and static filtering of the ESKF based on INS/GNSS/UWB. Sensors. 2023;23(10):4735. doi:10.3390/s23104735. [Google Scholar] [PubMed] [CrossRef]

14. Luo J, Yin Z, Gui L. A GNSS UWB tight coupling and imu eskf algorithm for indoor and outdoor mixed scenario. Clust Comput. 2024;27(4):4855–65. doi:10.1007/s10586-023-04208-2. [Google Scholar] [CrossRef]

15. Brigadnov I, Lutonin A, Bogdanova K. Error state extended Kalman filter localization for underground mining environments. Symmetry. 2023;15(2):344. doi:10.3390/sym15020344. [Google Scholar] [CrossRef]

16. Davari N, Gholami A, Shabani M. Multirate adaptive Kalman filter for marine integrated navigation system. J Navigation. 2017;70(3):628–47. doi:10.1017/s0373463316000801. [Google Scholar] [CrossRef]

17. Chen P, Li P, Ji B, Wang S, Song S, Yan T, et al. Integrated underwater navigation algorithm for AUV based on improved Sage-Husa and iterated maximum mixture correntropy unscented Kalman filter. Ocean Eng. 2025;341(14):122610. doi:10.1016/j.oceaneng.2025.122610. [Google Scholar] [CrossRef]

18. Jiang W, Tan X. Low cost inertial sensor attitude fixation algorithm and accuracy analysis. Adv Space Res. 2023;72(6):2270–82. doi:10.1016/j.asr.2023.06.039. [Google Scholar] [CrossRef]

19. Pang H, Pan M, Chen J, Li J, Zhang Q, Luo S. Integrated calibration and magnetic disturbance compensation of three-axis magnetometers. Measurement. 2016;93(6):409–13. doi:10.1016/j.measurement.2016.06.056. [Google Scholar] [CrossRef]

20. Wei X, Fan S, Zhang Y, Gao W, Shen F, Ming X, et al. A robust adaptive error state Kalman filter for MEMS IMU attitude estimation under dynamic acceleration. Measurement. 2025;242:116097. doi:10.1016/j.measurement.2024.116097. [Google Scholar] [CrossRef]

21. Shi G, Li X, Jiang Z. An improved yaw estimation algorithm for land vehicles using MARG sensors. Sensors. 2018;18(10):3251. doi:10.3390/s18103251. [Google Scholar] [PubMed] [CrossRef]

22. Laidig D, Caruso M, Cereatti A, Seel T. BROAD—a benchmark for robust inertial orientation estimation. Data. 2021;6(7):72. doi:10.3390/data6070072. [Google Scholar] [CrossRef]