Adaptive State-Separated UFIR Filter for Attitude Estimation Using MARG Sensors

TL;DR

This paper introduces a new adaptive filter for estimating vehicle orientation using sensor data, which improves accuracy and efficiency.

Contribution

The novel ASSUFIR algorithm adapts different estimation horizons for separate states using quaternions and QR decomposition.

Findings

The ASSUFIR algorithm reduces computational burden by applying different horizons to different states.

QR decomposition maintains computational efficiency despite structural changes.

Experimental results show the method's effectiveness in real-world vehicle attitude estimation.

Abstract

Unbiased Finite Impulse Response (UFIR) filters are widely used in engineering applications, such as vehicle attitude estimation, due to their advantages, including independence from initial conditions and insensitivity to noise. However, the performance of the UFIR filter heavily relies on the estimation horizon N, and different states within the system may exhibit an inverse correlation with respect to N, affecting the estimation results. To address this issue, this paper proposes an adaptive state-separated UFIR (ASSUFIR) filtering algorithm based on the properties of quaternions. By leveraging the relationship between quaternions and attitude angles, the algorithm reduces the computational burden of the batch UFIR filter estimation system, allowing different horizon lengths to be applied to different states. To mitigate the computational efficiency loss caused by disrupting the original UFIR filter structure, QR decomposition is introduced. The algorithm is first validated using simulated data and then compared with classical methods using real vehicle data. Experimental results demonstrate the practical applicability of the proposed method in engineering applications.

Figures

Full text

1. Introduction

In recent years, driven by rapid advances in autonomous driving and intelligent mobility, accurate ground-vehicle attitude estimation (pitch, roll, and yaw) has become critical for navigation, perception, stability control, and safety functions in challenging operating environments [1,2,3,4,5,6]. In parallel, continued progress in micro-electro-mechanical systems (MEMS) has enabled lightweight inertial sensors and multi-sensor integration to be widely deployed for attitude estimation due to their favorable dynamic response and ease of integration [7,8,9,10].

A magnetic, angular-rate, and gravity (MARG) sensing unit, typically composed of an inertial measurement unit (IMU) and a triaxial magnetometer, offers a compact, low-power solution. The IMU provides high-rate angular-rate and specific-force measurements, while the magnetometer supplies a heading reference through the local geomagnetic field. In practice, however, sensor imperfections and environmental disturbances (e.g., gyroscope bias drift, maneuver-induced specific-force acceleration, and magnetic distortions) can substantially degrade estimation accuracy [11,12,13]. As a result, robust attitude estimation generally requires sensor fusion frameworks that integrate multi-modal information.

Research on MARG-based attitude estimation can be broadly categorized into (i) attitude reconstruction methods and (ii) filtering/observer-based methods. Reconstruction approaches estimate attitude by solving the Wahba problem using vector observations derived from accelerometer and magnetometer measurements [14,15,16]. Filtering-based approaches incorporate process models and measurement updates to produce smoothed, dynamically consistent attitude estimates [17,18].

Among filtering approaches, Kalman-filter-based methods are widely used due to their strong estimation performance. For linear systems with independent white Gaussian noise, the Kalman filter is statistically optimal and often converges rapidly in practice. To accommodate quaternion representations and address nonlinearity, various quaternion-based formulations and Kalman filter variants have been developed, including the extended Kalman filter (EKF) [19] and indirect Kalman filter (IKF) [20]. Computationally efficient variants, such as lightweight EKF, have also been proposed to reduce the cost of the update step [21], while multi-layer or cascaded architectures can improve robustness under modeling mismatch.

Nevertheless, Kalman-filter-based methods typically depend on accurate process or measurement models and reliable noise statistics [22,23,24], which are difficult to obtain in highly dynamic, disturbance-prone conditions common in ground-vehicle operation. To reduce reliance on noise statistics, finite-impulse-response (FIR) approaches have been explored. In particular, the unbiased FIR (UFIR) filter estimates the current state from a finite window of recent measurements and does not require explicit noise statistics or precise initial conditions [25]. A key design parameter is the horizon length. For time-invariant systems, $[eqn]$ can be selected by minimizing the mean-square error (MSE) under test conditions [26,27]. For time-varying systems, however, a fixed horizon may fail to track changing dynamics, motivating adaptive horizon-selection strategies. Existing studies have proposed maximum-likelihood-based selection [28] and filter-bank approaches for real-time horizon adjustment [29].

Building on our previous quaternion-based UFIR attitude estimation framework, we identify two practical issues in dynamic vehicle operation. First, because the state model is driven by gyroscope integration, the effective system behavior is time-varying; consequently, a single fixed horizon length N is only optimal at certain time instants. Second, empirical results indicate that the attitude components can exhibit conflicting sensitivity to N: for example, horizon lengths that improve estimation for one angle (e.g., pitch/roll) may degrade another (e.g., yaw), leading to a compromise when a single global horizon is used (see Figure 1).

Motivated by these observations, we propose a quaternion-based adaptive state-separated UFIR (ASS-UFIR) method. The main contributions are:

• (1)We reformulate the extended state-space model by exploiting quaternion properties, enabling different horizon lengths to be assigned to different attitude-related state components while maintaining a consistent quaternion representation (e.g., through appropriate constraint handling/normalization). This mitigates conflicting horizon sensitivity across attitude angles.
• (2)Empirically, the UFIR performance varies nonlinearly with N and often exhibits a local-optimal region in which small horizon changes have a limited effect. Based on this observation, we construct an offline admissible interval for N and map an estimated motion regime to this interval to update N online at each step, enabling adaptive selection with low tuning burden.
• (3)Because state-dependent horizons disrupt the standard iterative UFIR structure, we adopt a batch least-squares formulation. To reduce computation and avoid explicit matrix inversion, we implement batch updates using a QR-decomposition-based solver, thereby improving numerical stability and runtime efficiency for embedded systems.

The remainder of this paper is organized as follows. Section 2 reviews related work and presents the system model and notation used throughout the paper. Section 3 describes the proposed adaptive state-separated UFIR filtering method, including its design rationale and implementation. Section 4 develops an efficient QR-decomposition-based solution for the resulting batch formulation. Section 5 presents simulation and real-vehicle experimental results, and compares the proposed method with representative baseline approaches. Finally, Section 6 concludes the paper and discusses directions for future work.

2. Preliminaries

To support quaternion-based three-dimensional attitude representation, this section summarizes the quaternion definitions, notation, and key properties used throughout the paper, and then presents the mathematical models adopted in this study.

2.1. Attitude Representation

The attitude of a rigid body describes the orientation of the body-fixed coordinate frame b with respect to the navigation frame n. The b-frame is rigidly attached to the vehicle (or mobile robot) and its axes are aligned with the vehicle structure, whereas the n-frame serves as a reference frame aligned with the chosen navigation convention (e.g., a local-level geographic frame), providing an absolute directional reference.

The relative rotation between any two frames can be represented by a quaternion. Let the quaternion be expressed as

$$[eqn]$$

where $[eqn]$ denotes the real part and $[eqn]$ denotes the imaginary part. According to the quaternion rotation definition, it can be written as

$$[eqn]$$

where $[eqn]$ is the rotation angle, and $[eqn]$ is the unit vector representing the rotation axis. Because quaternions encode rotations, unit normalization is commonly enforced to avoid representation ambiguity and to simplify subsequent computation. Moreover, the conjugate corresponds to the opposite rotation as

$$[eqn]$$

2.2. Problem Formulation

To facilitate the discussion of the proposed method, it is essential to define the notations and coordinate systems used throughout this paper. The sensor coordinate system is designated as the body frame (b), and the global reference coordinate system is denoted as the navigation frame (n). The measurement of the accelerometer is defined as $[eqn]$ , and the gravitational acceleration is given by $[eqn]$ . In these expressions, $[eqn]$ , $[eqn]$ , and $[eqn]$ are the components of acceleration along the three axes of the body frame.

Measurements from MARG sensors are typically non-normalized vectors with differing units (e.g., meters per second squared for acceleration and Gauss for magnetic field strength). These inconsistencies in scale can pose challenges during data fusion. Similarly, the magnetometer measurement is defined as $[eqn]$ with $[eqn]$ . The reference vector for the horizontal component of the magnetic field is given by $[eqn]$ with $[eqn]$ . Finally, the measurement of the gyroscope is defined as $[eqn]$ .

2.3. Construction of the State-Space Model

In this study, we adopt the algebraic quaternion model proposed in [18] to formulate the system dynamics and measurements. The system state is denoted by the quaternion $[eqn]$ , where k and $[eqn]$ represent the current and previous time steps, respectively. Based on this model, the state and measurement equations are

$$[eqn]$$ $$[eqn]$$

Here, $[eqn]$ represents the state transition matrix, obtained from the quaternion differential equation via a first-order discretization [30]

$$[eqn]$$

where $[eqn]$ denotes the sampling interval. The operator $[eqn]$ is the quaternion-rate matrix constructed from $[eqn]$ as

$$[eqn]$$

And $[eqn]$ is the observation matrix and is set to the identity matrix. The vector $[eqn]$ denotes the measurement, and $[eqn]$ is the algebraic quaternion derived analytically from the accelerometer and magnetometer [18]. The specific derivation process is not the focus of this article, and the conclusion is estimated to be directly given here:

$$[eqn]$$

Among them, ⊗ denotes quaternion multiplication, and $[eqn]$ and $[eqn]$ are quaternions obtained through algebraic analysis of measurement data from accelerometers and magnetometers, respectively. The terms $[eqn]$ and $[eqn]$ are the process and measurement noise, respectively.

3. Adaptive State-Separated UFIR Filter

3.1. State-Separated UFIR Filter Design

In our previous work, we reformulated a quaternion-based linear attitude estimation model from the KF framework into the UFIR framework and experimentally validated improved robustness. However, we observed that the horizon length can affect different state components in different (and sometimes opposing) ways. Consequently, using a single global horizon for all components may yield only locally optimal performance for certain attitude components while degrading others. To address this issue, we propose a state-separated UFIR (SS-UFIR) method based on an extended state-space formulation. By partitioning the estimation problem into reduced sub-models associated with different state components, the proposed approach allows each component (or component group) to employ a distinct horizon length N. This design aims to improve overall attitude estimation accuracy across all components. The structural difference between the conventional global-horizon strategy and the proposed state-separated strategy is illustrated in Figure 2. Consider the discrete-time linear model (without control input) introduced above. For a given time index k, define the horizon $[eqn]$ , with $[eqn]$ , so that the horizon contains N consecutive samples. The specific value of N can be referred to [25]. The corresponding extended state-space model over $[eqn]$ can then be written as follows:

$$[eqn]$$ $$[eqn]$$

The components of the extended state-space system are defined as follows:

$$[eqn]$$ $$[eqn]$$ $$[eqn]$$ $$[eqn]$$ $$[eqn]$$ $$[eqn]$$ $$[eqn]$$

in which $[eqn]$ . Subsequently, according to the unbiasedness condition and zero-input response, we define the $[eqn]$ . Due to the system defined in this study having no input, the generalized noise power gain (GNPG) [25] is given by

$$[eqn]$$

The batch UFIR filter then calculates the state estimates at time epoch k as follows:

$$[eqn]$$

By substituting $[eqn]$ into the equation and rearranging the terms, the expression can be written in the standard normal equation form as follows:

$$[eqn]$$

According to the characteristics of our model, since the system has no input and the measurement matrix is an identity matrix, expanding the above equation yields a homogeneous linear overdetermined system with a size of N × 4 (where 4 is the state dimension), as shown below:

$$[eqn]$$

We now return to the definition of quaternions. According to their mathematical formulation, the four elements of a quaternion respectively represent the rotation angle and the orientation of the rotation axis along three spatial directions. In the defined local coordinate frame, the last three components can be interpreted as the rotational components corresponding to the body’s three principal axes. Hence, these imaginary components can be generally regarded as carrying the primary rotational information of the rigid body’s attitude. Based on this understanding, the final attitude angles estimated by the filter are mapped onto the system state, enabling a generalized separation among the three attitude angles.

When incorporating this idea into the UFIR-based attitude estimation process, we introduce two horizon lengths, $[eqn]$ and $[eqn]$ , to reflect different filtering requirements. Let $[eqn]$ and $[eqn]$ . The larger horizon $[eqn]$ is used for state components that benefit from stronger noise suppression and smoother estimates, whereas the smaller horizon $[eqn]$ is used for components requiring faster dynamic response.

In the model considered in this study, as shown in Equations (6) and (7), the measurements from the gyroscope form the state transition equation and have already been integrated into the system’s prior calculation as part of the state prediction. On the other hand, as shown in Equation (8), the observation quaternion is constructed from the measurements of the accelerometer and magnetometer, and they are fused through quaternion multiplication. Before fusion, the decomposed quaternion definitions are as follows: $[eqn]$ represents the rotation around the z-axis, meaning that the magnetometer is primarily used to provide heading reference. The accelerometer, on the other hand, is used to provide pitch and roll information. Therefore, in subsequent analysis, we group pitch and roll together, assigning them the horizon $[eqn]$ . Yaw is treated separately, using the horizon $[eqn]$ .

Under the adopted small-angle (or linearized) quaternion model, these groupings correspond to assigning $[eqn]$ to the roll/pitch-related quaternion components (e.g., $[eqn]$ and $[eqn]$ ) and to the yaw-related component (e.g., $[eqn]$ ). During the batch UFIR solution at time step, the stacked equation system can be truncated according to the horizon associated with each state group, i.e., using measurements over $[eqn]$ for the roll/pitch group and $[eqn]$ for the yaw group. To reduce the start-up delay, we initialize the horizon as $[eqn]$ , enabling an earlier valid estimate and shortening the initial warm-up period. Index stacked rows in time major order by

$$[eqn]$$

where $[eqn]$ , and the time set retained by each dimension is

$$[eqn]$$

where $[eqn]$ denotes the set of horizons in the c-th dimension of a quaternion, $[eqn]$ . Mapping these time sets into a set of row numbers and merging them together is the preserved set of rows:

$$[eqn]$$

The number of reserved rows (i.e., the number of equations after clipping):

$$[eqn]$$

Subsequently, construct the trimming (row-selection) matrix $[eqn]$ by stacking standard-basis row vectors. Let $[eqn]$ be the standard basis of $[eqn]$ , where $[eqn]$ is the column vector with a single 1 in the i-th position and zeros elsewhere. Sort the elements of $[eqn]$ in ascending order as $[eqn]$ , where $[eqn]$ represents the j-th element of the set, and M is the total number of reserved rows. Then, $[eqn]$ is constructed as follows:

$$[eqn]$$

By construction, left-multiplication simply picks those rows:

$$[eqn]$$ $$[eqn]$$

Then, obtain the estimation result by solving the following equation:

$$[eqn]$$

The diagram of the trimming process is shown in the Figure 3. Four different colors represent the quaternion components at different time steps, and the red lines indicate the trimmed sections. The figure uses yaw as an example with a small window.

3.2. Adaptive Horizon Length

As mentioned in [25], the only tuning parameter in the UFIR filter algorithm is the horizon length N, and its optimal selection is critical for minimizing the system’s MSE. Some studies suggest that, when test measurements are available, N can be optimized by minimizing the trace of the system’s error covariance matrix. Another approach, as discussed in [26], is to estimate N using the measurement values by minimizing the residual covariance matrix. However, these methods are primarily applicable to time-invariant systems. For time-varying systems, N must be dynamically adjusted at each time index k.

The constructed system, as shown in the (4) and (5), features a state transition matrix derived from the gyroscope’s differential equation. The core structure is based on the gyroscope measurements, classifies it as time-varying. Therefore, to maintain the estimation performance of the system, the value of N must be dynamically updated at each time step.

In the past, several methods for adaptively selecting have been proposed, mainly based on maximum likelihood estimation or the UFIR bank approach. The essence of these methods is to set an evaluation metric at each time step and compare the system’s estimation results under different values of N, selecting the optimal outcome. For some embedded devices, these methods are time-consuming and require substantial memory, which may not be feasible due to memory constraints. To address this, this paper proposes a novel method for adaptive adjustment of N based on a linear mapping range.

As shown in the Figure 1, the relationship between the system’s estimation performance and N is nonlinear, and the effect of local variations of N on the system performance can be almost negligible. Based on this characteristic, we determine an acceptable range for N offline, namely $[eqn]$ , where $[eqn]$ and $[eqn]$ are the minimum and maximum values derived from offline testing.

Next, based on the system motion characteristics, we define two angular–rate thresholds to the different state components, $[eqn]$ and $[eqn]$ , where $[eqn]$ denotes the pitch and roll threshold, and $[eqn]$ characterizes yaw variations. Instead of relying on a single-step difference, we first compute the instantaneous angular rates at each sample and then average them over the window of length N:

$$[eqn]$$ $$[eqn]$$

These averaged rates are then mapped to the UFIR horizons:

$$[eqn]$$

where $[eqn]$ denotes the pitch or yaw. At the same time, we also need to normalize the mapping of proportions to between $[eqn]$ in our calculations. Subsequently, we obtained two different horizons for trimming the number of equations in the UFIR filter. This update scheme can ensure the required dynamism of the window for time-varying systems, which helps to adaptively adjust the window size of the UFIR filter and optimize its performance.

4. QR Decomposition

As described in Section 3, introducing state-dependent horizon lengths enables state separation by reducing and restructuring the original UFIR estimation system. However, using multiple horizons breaks the shift-invariant structure exploited by the standard low-complexity iterative UFIR recursion. In particular, for the state group associated with the larger horizon $[eqn]$ , the corresponding stacked system contains an initial block of rows for which the required past information is not available (i.e., the recursion cannot be initialized in the same manner as in previous iterative UFIR formulations). Therefore, we adopt the batch UFIR formulation for attitude estimation. The batch formulation requires solving a large-scale overdetermined linear system at each time step (or over each sliding horizon). A direct solution based on explicit matrix inversion, e.g., forming and inverting the normal matrix, incurs substantial computational cost and may suffer from numerical ill-conditioning, especially on platforms with limited compute resources. Consequently, an efficient solver that avoids explicit inversion while maintaining estimation accuracy is needed. This motivates the use of QR decomposition in this study [31].

Following Equation (29), after stacking the windowed equations and applying the state-separation (row-trimming) operation, the reduced overdetermined system can be written as follows:

$$[eqn]$$

Since $[eqn]$ in general, Equation (33) is solved in the least-squares sense, yielding the batch UFIR estimate

$$[eqn]$$ $$[eqn]$$

which is consistent with Equation (29). Although (35) provides a closed-form solution, explicitly forming and inverting $[eqn]$ is computationally demanding and may degrade numerical robustness when the stacked system becomes ill-conditioned. To avoid explicit inversion, we perform QR factorization on the specific matrix in Equation (29), i.e., $[eqn]$ :

$$[eqn]$$

where $[eqn]$ is orthogonal ( $[eqn]$ ) and $[eqn]$ is upper trapezoidal. Substituting (36) into (34) and using the invariance of the ℓ2-norm under orthogonal transforms, we obtain the following:

$$[eqn]$$

Let $[eqn]$ . Since $[eqn]$ has only 4 columns, the QR factorization yields $[eqn]$ with an upper-trapezoidal structure: the first 4 rows form an effective $[eqn]$ upper-triangular block, denoted by $[eqn]$ , whereas the remaining $[eqn]$ rows are reduced to (approximately) zero during the elimination. Accordingly, $[eqn]$ is partitioned conformably as $[eqn]$ , where $[eqn]$ consists of the first four elements of $[eqn]$ and is used together with $[eqn]$ to form a back-substitutable triangular system, while $[eqn]$ collects the remaining elements and represents the component lying in the orthogonal complement of the column space of $[eqn]$ after the orthogonal transform (i.e., the part associated with the least-squares residual). Therefore, $[eqn]$ can be written as

$$[eqn]$$

and the least-squares problem is equivalently reduced to solving the following upper-triangular linear system:

$$[eqn]$$

from which the quaternion estimate $[eqn]$ is obtained by back substitution.

5. Validations

5.1. Numerical Simulation

Before conducting experiments, we first validate the proposed algorithm through numerical simulations and compare it with the traditional UFIR filter algorithm. The true attitude $[eqn]$ is generated by adding random noise to the Equation (4). Then, simulated accelerometer and magnetometer measurements were derived based on the true geomagnetic field and gravitational acceleration values provided by, respectively,

$$[eqn]$$ $$[eqn]$$

where $[eqn]$ is the true attitude obtained from the simulation, $[eqn]$ and $[eqn]$ represent the measurement noise from the accelerometer and magnetometer, respectively. The noise characteristics are modeled as follows: Gaussian white distribution. In subsequent simulations, the noise density is set $[eqn]$ , $[eqn]$ , and $[eqn]$ . These values were determined based on empirical observations of the noise characteristics in the MARG sensor readings used in subsequent experiments. Specifically, the sensor was kept stationary, and multiple static measurements were collected, each lasting 10 min. The data from each collection was combined with the calibration noise values provided by the sensor’s factory specifications, and the average values were calculated. Then, the system state is converted into the attitude angle form through the following transformation:

$$[eqn]$$ $$[eqn]$$ $$[eqn]$$

where $[eqn]$ respectively represent roll, pitch, and yaw.

First, we simulated a rotation with the same magnitude of motion, where the angular velocities along the x-, y-, and z-axes were randomly defined within the range $[eqn]$ rad/s. The sampling frequency was set to 100 Hz, and the initial quaternion state was set as $[eqn]$ , with a 10-s simulation performed. During the testing phase, a representative set of simulated data was selected, and different horizon window sizes N were evaluated. For each candidate N, the corresponding UFIR estimation was performed, and the RMSE values of roll/pitch and yaw were computed, respectively. The optimal horizon for roll and pitch was chosen as the one that yielded the minimum RMSE, resulting in $[eqn]$ , while the optimal horizon for yaw was selected in the same manner, giving $[eqn]$ . These optimal horizon settings were then adopted in the subsequent algorithm comparisons. Therefore, traditional batch UFIR filters with fixed $[eqn]$ and $[eqn]$ were compared as benchmarks, and the results are shown in Figure 4 and Figure 5.

First, this simulation is designed not only for a statistical comparison of the final estimation results, but more importantly, to explicitly demonstrate how characteristic behaviors observed in the quaternion states are propagated to the attitude outputs through the quaternion-to-Euler mapping. Therefore, we present both the estimated state quaternions and the corresponding Euler-angle curves only in Figure 4 and Figure 5. Specifically, when a larger fixed horizon is adopted (e.g., UFIR-29), the estimation curves of $[eqn]$ and $[eqn]$ exhibit a pronounced lag, and the same lag is consistently reflected in the roll and pitch estimates; this lag is also observed in the subsequent simulations and experiments. In contrast, when a shorter horizon is used (e.g., UFIR-5), the $[eqn]$ component shows evident oscillations, which are mapped to the yaw channel and thus result in oscillatory yaw estimates. These results further verify the feasibility of the quaternion-to-Euler mapping for interpreting estimation behaviors. The above observations suggest that the three Euler-angle channels exhibit different sensitivities to the choice of the horizon length N, i.e., their estimation behaviors and error trends do not vary with N in a uniform manner. Consequently, adopting a single fixed-N UFIR filter may lead to an imbalanced performance across roll, pitch, and yaw, depending on the motion profile and measurement conditions. By contrast, the proposed SS-UFIR alleviates this intrinsic trade-off by assigning different horizon lengths to different state groups, thereby achieving a more balanced estimation behavior across all three axes.

To further quantify the overall attitude accuracy beyond the channel-wise Euler-angle RMSEs, we additionally introduce a global metric, namely the rotation angle (RA) error. Let $[eqn]$ and $[eqn]$ denote the reference and estimated unit quaternions at time step k, respectively, and define the relative quaternion as $[eqn]$ , where ⊗ denotes quaternion multiplication. The rotation angle error is then defined as

$$[eqn]$$

where $[eqn]$ is the scalar part of $[eqn]$ (the absolute value removes the sign ambiguity of unit quaternions). We report the RMSE of $[eqn]$ as a global performance indicator.

This conclusion is consistent with the quantitative results in Table 1: SS-UFIR achieves low roll/pitch errors comparable to UFIR-5 while maintaining a yaw error comparable to UFIR-29, thereby demonstrating superior overall performance.

Subsequently, the angular velocities along the x- and y-axes are defined as random values within the range $[eqn]$ rad/s, while the angular velocity along the z-axis is defined as random values within the range $[eqn]$ rad/s. This setup is designed to better simulate the actual motion scenarios of vehicles or mobile robots during the attitude estimation process. In this simulation, we introduced the method of adaptive adjustment of N based on the angular velocities and compared it with the traditional algorithm. The optimal values for the small and large windows were found to be 5 and 35, respectively.

As shown in Figure 6, the simulated vehicle-dynamics results indicate that the estimation behaviors of different attitude channels respond differently to the choice of horizon length N. Table 2 summarizes the Euler-angle RMSEs together with a global metric. For the fixed-horizon baselines, UFIR-5 achieves low roll/pitch errors (0.201°/0.190°) but yields relatively larger yaw (3.187°). Increasing the horizon (UFIR-37) slightly reduces the yaw error to 3.165°, yet it introduces a clear degradation in roll and pitch (0.330°/0.333°) and does not improve the global RA. By contrast, the proposed SS-UFIR provides a more favorable overall trade-off by assigning different horizon lengths to different state groups. As reported in Table 2, SS-UFIR preserves the low roll/pitch errors comparable to UFIR-5 (0.201°/0.190°), while reducing the yaw RMSE to 3.152° and the global RA to 3.161°, thereby indicating an overall improvement at the attitude level. Moreover, after incorporating the adaptive horizon mechanism, the same accuracy is achieved, which supports the effectiveness of the adaptive horizon selection in this setting.

Finally, Table 2 also reports the computation time, highlighting the advantage of the QR-based solver. Although ASS-UFIR/SS-UFIR require solving an overdetermined system at each time step, explicitly forming and inverting the normal matrix is unnecessary. By employing QR decomposition implemented via Givens rotations, ASSQR-UFIR reduces the computation time from $[eqn]$ to $[eqn]$ , corresponding to an improvement of approximately $[eqn]$ , while maintaining identical RMSEs and RA to ASS-UFIR.

5.2. Field Experiment

We evaluated the proposed adaptive state-separated QR-UFIR (ASSQR-UFIR) on approximately 800 s of real vehicle data; the raw IMU and magnetometer measurements are shown in Figure 7. Attitude estimation accuracy was assessed against a reference attitude obtained from an attitude and heading reference system (AHRS), and the attitude error was computed as Euler-angle differences. For benchmarking, we compared against the Madgwick gradient-descent attitude (GDA) filter, Mahony’s SO(3) complementary filter, AQUA-KF, and the conventional batch UFIR filter. For AQUA-KF, the initial quaternion and covariance were set to $[eqn]$ and $[eqn]$ , and the process/measurement noise parameters were set according to the sensor datasheet. The Madgwick gain was set to $[eqn]$ following [32], and the Mahony proportional gain was set to $[eqn]$ following the implementation in [33]. For the batch UFIR baseline, we report results for fixed horizons $[eqn]$ and $[eqn]$ . For ASSQR-UFIR, we used $[eqn]$ and $[eqn]$ , with $[eqn]$ and $[eqn]$ .

The estimated attitudes and the corresponding errors are illustrated in Figure 8 and Figure 9, and Table 3 reports the RMSEs, the proposed global metric, and the runtime. Overall, ASS-UFIR provides the most favorable accuracy profile in this experiment: it achieves consistently low RMSEs in roll, pitch, and yaw, and simultaneously yields the smallest RA RMSE (6.923°), indicating superior overall attitude consistency on $[eqn]$ rather than only channel-wise improvements. Compared with fixed-horizon UFIR baselines, the state-dependent horizon design alleviates the imbalance caused by differences in the attitude channels’ sensitivities to horizon length and improves estimation quality across the full dataset.

Regarding computational cost and memory footprint, it should be emphasized that our efficiency discussion is primarily based on comparisons between the proposed QR-based solver and its corresponding non-QR implementation under the ASS-UFIR formulation, rather than against traditional baseline filters with fundamentally different update structures. As shown in Table 3, replacing the baseline batch solver in ASS-UFIR with the Givens-rotation QR solver reduces the runtime from $[eqn]$ to $[eqn]$ , i.e., an improvement of approximately $[eqn]$ , while preserving identical estimation accuracy (including the RA metric). Moreover, the QR factorization avoids explicitly forming and inverting the normal matrix and enables an in-place triangular update, thereby reducing the peak storage requirement when solving the overdetermined batch system. In addition, by restructuring the estimation system via state separation, the maximum batch system dimension is reduced by up to $[eqn]$ relative to the unreduced formulation, further lowering memory demand. These benefits, together with initializing the horizon using $[eqn]$ and $[eqn]$ to shorten the initial warm-up period (instead of starting directly from $[eqn]$ ), support the practical feasibility of deploying the proposed method on resource-limited platforms.

6. Conclusions

This paper proposed an adaptive-state-separated UFIR attitude estimation method for MARG sensors to address the practical limitation that a single UFIR horizon length N can be suboptimal when different attitude angles exhibit different (and sometimes conflicting) sensitivity to the horizon. In particular, roll and pitch are primarily constrained by the gravity direction measured by the accelerometer, whereas yaw relies more strongly on geomagnetic heading from the magnetometer, motivating-state-dependent horizon allocation. Using a quaternion-based formulation, we group the attitude-related states and assign distinct horizon windows; Euler-angle quantities are used only to guide the grouping/adaptation while maintaining a consistent quaternion representation. To accommodate time-varying vehicle dynamics, an angle-guided mechanism is further introduced for online horizon updating. Simulation and field experiments show that the proposed method achieves improved overall attitude accuracy relative to representative baselines across the full dataset, as reflected in both channel-wise RMSEs and the global RA metric. Moreover, within the ASS-UFIR formulation, replacing the baseline batch solver with a Givens-rotation QR solver reduces runtime without altering estimation accuracy, and the reduced-batch formulation reduces the size of the stacked overdetermined system to be solved, facilitating efficient implementation on resource-limited platforms. Future work will extend the approach to more complex models and operating conditions with stronger disturbances and broader motion regimes.