Error State Kalman Filter for Integrated Attitude Estimation Based on Data Fusion of MIMU Inertial Array and Magnetometer

Abstract

Attitude estimation has increasingly relied on MEMS inertial measurement units (IMUs) owing to the low cost and miniature size, but the inherent high random noise and error accumulation limit long-term measurement accuracy. This article proposes an integrated attitude estimation algorithm based on data fusion from a MIMU inertial array and magnetometer to address this challenge. First, a redundant inertial array is constructed using homogeneous gyroscopes, and a Kalman filter (KF) is designed to fuse output signals from multiple gyroscopes to estimate true angular rate. Second, an integrated error state Kalman filter (ESKF) for the MIMU/magnetometer system is developed. Using the attitude quaternion calculated by the strapdown inertial solution as the nominal state and combining it with measurements from the accelerometer and magnetometer as observations, the attitude error is estimated and corrected. Both simulations and field experiments were conducted to validate the effectiveness of the proposed algorithm. The experimental results show that the ESKF algorithm performs best in estimation accuracy and addresses the issue of error accumulation and fluctuation. In particular, the Root Mean Square Error (RMSE) of the ESKF algorithm was significantly reduced, with the roll angle reduced by 55.34%, the pitch angle by 30.25%, and the yaw angle by 55.71%.

1. Introduction

Accurate and high-speed attitude estimation has emerged as a paramount prerequisite, driven by the booming proliferation of autonomous systems like robots, unmanned aerial vehicles (UAVs), and unmanned ground vehicles (UGVs) [1]. Recent years have witnessed substantial enhancements in the precision and dependability of low-cost micro inertial measurement units (MIMUs), driven by the continuous evolution of MEMS fabrication processes [2]. Due to cost and resource constraints, high-precision inertial navigation systems are not suitable for such scenarios. For attitude measurement in unmanned systems, the low-cost MEMS IMU remains the most suitable option. A MEMS IMU consists of a three-axis gyroscope and a three-axis accelerometer, providing comprehensive navigation data. A vibrating MEMS gyroscope detects the Coriolis force induced by angular motion, which is measured as a capacitance change proportional to the angular rate. A accelerometer measures acceleration via the capacitance change induced by the displacement of its proof mass under external force. The MEMS IMU offers a high degree of autonomy and stealth and is resistant to external disturbances [3]. The gyroscope obtains real-time attitude angle information by measuring and integrating the angular rate of the platform, while the accelerometer can also determine the platform’s attitude with the assistance of the gravity vector. Nevertheless, inherent constraints in MEMS fabrication processes result in substantially elevated random noise levels in MEMS gyroscopes compared to fiber-optic and laser counterparts. Consequently, the rapid accumulation of errors during attitude estimation renders them inadequate for prolonged operational tasks [4]. MEMS accelerometers do not suffer from cumulative integration errors during long-term attitude estimation, but their measured values represent the specific force acting on the platform, making them susceptible to dynamic acceleration, which can cause attitude estimation errors to diverge.

To address the above problems, two approaches are commonly used: improving the accuracy of the MEMS IMU and multi-sensor data fusion [5]. The overall measurement accuracy of an IMU is predominantly dictated by the angular rate accuracy of its MEMS gyroscope. MEMS gyroscopes typically contain rotating or vibrating components that measure angular rate using the Coriolis effect, making them more susceptible to noise and disturbance. The implementation of gyro array technology represents a potent strategy for enhancing the measurement accuracy of MEMS gyroscopes. Compared to other high-precision devices, it is low-cost and lightweight, making it suitable for resource-constrained unmanned platforms. Gyro array technology was first proposed by Bayard et al. in 2003 [6]. The underlying principle of this method relies on utilizing KF to combine the outputs of redundant MEMS gyroscopes, thereby generating a virtual gyroscope with optimized performance. Literature [7] used a Kalman filter to fuse multiple uncorrelated MEMS gyroscopes. By adjusting the parameter $q_{\omega }$, the accuracy was improved beyond that of simple averaging. The method was validated using six gyroscopes in experiments, resulting in a significant reduction in noise and bias instability. Literature [8] proposes a statistical method for analyzing the actual correlation coefficients of a MEMS gyro array to address the problem of determining the noise covariance matrix. Experiments with a four-gyro array show that negative noise correlation can improve accuracy. In [9], a two-stage decoupling fusion strategy for unknown disturbances was proposed for the first time. Through local estimation and global weighted fusion, bias accuracy is maintained under disturbances. In [10], the measurement noise covariance of the virtual gyroscope is optimized by employing an improved Sage-Husa adaptive Kalman filter. Literature [11] introduces a novel fusion strategy that surpasses the traditional accuracy limit associated with constant weight fusion. By dynamically adjusting weights, this approach effectively reduces standard deviation without compromising bandwidth. The gyroscope array has been extensively studied as an effective approach to resolving the trade-off between accuracy and cost. Other related techniques involve long short-term memory (LSTM) networks [12,13], extreme learning machines [14], attention mechanisms [15], and reinforcement learning [16]. Currently, there has been little research on gyroscope arrays used directly for navigation. Although the gyroscope array can reduce random noises through KF, its residual noise cannot be eliminated, and the resulting error accumulation must therefore be corrected using external references.

Multi-sensor data fusion is another key method for addressing the accumulation of attitude errors and improving system accuracy and robustness. Currently, there are various combinations of inertial sensors with GNSS, magnetic sensors and other types of sensors. Given the differences in characteristics of MEMS IMUs and magnetic sensors, integrating inertial information and magnetic field measurements using fusion algorithms typically yields better attitude estimation accuracy and stability. The most commonly used sensors are accelerometers and magnetometers, which provide a relatively stable long-term reference for correcting the integration error in attitude estimation. Literature [17] proposes a segmented angular rate estimation method using the IMU array. An extended Kalman filter is used for low dynamic conditions, while the Levenberg–Marquardt iterative method is used for high-dynamic conditions. To mitigate magnetic noise coupling, Literature [18] presented a novel decoupled extended KF, which utilizes dual independent quaternions to estimate attitude and heading, respectively. Literature [19] detects magnetic field trend changes using a sliding window to determine magnetometer data fusion, significantly improving attitude accuracy and stability under magnetic disturbances. Literature [20] combines angular accelerometers with low-cost gyroscopes and uses deep learning to automatically learn optimal noise covariance parameters from raw data. Literature [21] uses a nine-axis IMU to estimate attitude and proposes a magnetic disturbance detection and adaptive gradient descent algorithm based on quaternion similarity. These methods all leverage the complementary strengths of heterogeneous sensors to improve the accuracy of attitude estimation. A nine-axis MEMS IMU typically includes a gyroscope, accelerometer and magnetometer, which can provide comprehensive navigation data, including angular rate, acceleration, and geomagnetic field measurements. By designing an appropriate fusion algorithm, the accuracy of attitude estimation can be significantly improved. An extensive literature review reveals that most existing studies address isolated aspects, such as optimizing only the array fusion strategy or improving only the navigation algorithm, while few integrate the full pipeline and validate it through real-world experiments.

To address the challenges of cost and resource-constrained unmanned systems, this paper introduces a data fusion algorithm that combines a MIMU inertial array with a magnetometer for robust attitude estimation. First, a Kalman filter is applied to fuse the outputs of the redundant sensor array, which is built using homogeneous gyroscopes, in order to estimate the true angular rate. In the second stage, a fusion algorithm based on an error state Kalman filter is established for the MIMU/magnetometer system, aiming to determine the quaternion error corresponding to the strap-down inertial navigation solution. Finally, attitude estimation with higher accuracy is achieved, and the performance of the integrated ESKF algorithm regarding signal fusion and attitude measurement is validated through comprehensive simulations and experiments.

2. Data Fusion Algorithm for Inertial Array

2.1. Sensor Error Models for Integrated System

The measurements from MEMS gyroscopes contain various types of random error, such as Angular Random Walk (ARW) and Rate Random Walk (RRW). For measurement error models, while incorporating more error terms can certainly improve accuracy, it also increases model complexity, which is not conducive to the subsequent design of filtering algorithms. The predominant sources of stochastic error, as identified through the evaluation of actual measurement data, are angle random walk and rate random walk. Therefore, the error model for MEMS gyroscopes considering ARW and RRW can be written as:

$$\left\{\begin{array}{c}{}^b\mathit{\omega }_i^m={\mathit{\omega }}^r+{\mathit{b}}_i^{\omega }+{\mathit{n}}_i^{\omega }\hfill \\ {\dot{\mathit{b}}}_i^{\omega }={\mathit{w}}_{b_i}\hfill \end{array}\right.$$ (1)

where ${}^b\mathit{\omega }_i^m$ is the output signal of ith gyroscope in body coordinate system, ${\mathit{\omega }}^r$ is the true angular rate, ${\mathit{b}}_i^{\omega }$ is rate random walk, ${\mathit{n}}_i^{\omega }$ is Gaussian white noise representing angle random walk, and ${\mathit{w}}_{b_i}$ is the driving Gaussian white noise for rate random walk.

To directly estimate the true angular rate, $\omega$ must be modeled. To capture the dynamic evolution of the true angular rate, a first-order Markov formulation is utilized:

$${\dot{\mathit{\omega }}}^r=-{\displaystyle \frac{1}{\tau }}·{\mathit{\omega }}^r+{\mathit{n}}_{\omega },$$ (2)

where $\tau$ is the correlation time constant, ${\mathit{n}}_{\omega }$ contains Gaussian white noises with variance ${\mathit{q}}_{\mathit{\omega }}$, and the magnitude of ${\mathit{q}}_{\mathit{\omega }}$ characterizes the dynamic variation of input angular rate.

In addition to the redundant gyro array used to measure angular rate, the system also includes an accelerometer and magnetometer. The accelerometer is used to measure the linear acceleration of the carrier, and the magnetometer is used to measure the magnetic field strength of the surrounding environment. This forms the MIMU/magnetometer system that provides comprehensive data for attitude calculation. The error models for the accelerometer and magnetometer are presented below.

$${}^b\mathit{a}^m={\mathit{a}}^r+{\mathit{n}}^a+\mathit{g},$$ (3)

$${}^b\mathit{m}^m={\mathit{m}}^r+{\mathit{b}}^m+\mathit{d},$$ (4)

where ${}^b\mathit{a}^m$ and ${}^b\mathit{m}^m$ represent the measurements from the accelerometer and magnetometer, ${\mathit{a}}^r$ and ${\mathit{m}}^r$ are the true reference of the accelerometer and magnetometer, ${\mathit{n}}^a$ is the Gaussian white noise, ${\mathit{b}}^m$ is the bias error of the magnetometer, $\mathit{g}$ is the gravity acceleration, $\mathit{d}$ is the magnetic disturbance vector. The accelerometer and magnetometer output measurement signals after calibration, which are normalized for subsequent attitude estimation.

2.2. Filter Algorithm Design for MIMU Array Fusion

Fusing the three-axis angular rate output signals from the inertial array is key to the first-stage filter algorithm, which is used to suppress random noise in the measurement signals and reduce error accumulation during attitude estimation. Multiple gyroscopes are installed in a parallel configuration, and installation errors have been compensated for through calibration. Using the Kalman filter architecture, for an inertial array containing N gyroscopes, the true angular rate ${\mathit{\omega }}^r$ relative to the carrier frame and the gyroscope zero bias are selected to form the state vector:

$${\mathit{X}}_1={\left[{\left({\mathit{\omega }}^r\right)}^T,{\mathit{b}}_1^T,{\mathit{b}}_2^T,\dots ,{\mathit{b}}_N^T\right]}^T\in {\mathbb{R}}^{3+3N},$$ (5)

where ${\mathit{\omega }}^r={[{\mathit{\omega }}_x^r,{\mathit{\omega }}_y^r,{\mathit{\omega }}_z^r]}^T$ is true angular rate vector, ${\mathit{b}}_i={[b_{ix},b_{iy},b_{iz}]}^T$ is the bias vector of ith gyro. Noise vector ${\mathit{n}}_{\mathit{\omega }}=[n_{\omega x},n_{\omega y},n_{\omega z}]$ denotes as the driving noise of ${\mathit{\omega }}^b$.

Based on the error model established, by treating the gyro array output as the measurement for KF, the state-space model can be obtained as follows:

$$\left\{\begin{array}{c}{\dot{\mathit{X}}}_1\left(t\right)={\mathit{F}}_1·{\mathit{X}}_1\left(t\right)+{\mathit{W}}_1\left(t\right)\hfill \\ {\mathit{Z}}_1\left(t\right)={\mathit{H}}_1·{\mathit{X}}_1\left(t\right)+{\mathit{V}}_1\left(t\right)\hfill \end{array}\right.,$$ (6)

where ${\mathit{F}}_1$ and ${\mathit{H}}_1$ are coefficient matrices, the measurement vector ${\mathit{Z}}_1\left(t\right)$ consists of the three-axis output signals from the gyro array, process noise ${\mathit{W}}_1={[{\mathit{n}}_{\omega }^T,{\mathit{w}}_{b1}^T,\dots ,{\mathit{w}}_{bN}^T]}^T$, ${\mathit{w}}_{bi}^T=[w_{bix},w_{biy},w_{biz}]$, measurement noise ${\mathit{V}}_1={[{\mathit{v}}_1^T,\dots ,{\mathit{v}}_N^T]}^T$, ${\mathit{v}}_1^T={[n_{ix},n_{iy},n_{iz}]}^T$, all noise vectors are Gaussian white noise and satisfy the following relationship:

$$\left\{\begin{array}{c}E\left[{\mathit{W}}_1\left(t\right){\mathit{W}}_1^T(t+\tau )\right]={\mathit{Q}}_1\delta \left(\tau \right)\hfill \\ E\left[{\mathit{V}}_1\left(t\right){\mathit{V}}_1^T(t+\tau )\right]={\mathit{R}}_1\delta \left(\tau \right)\hfill \end{array}\right.,$$ (7)

where ${\mathit{Q}}_1$ and ${\mathit{R}}_1$ are noise covariance matrices.

The coefficient matrices ${\mathit{F}}_1$ and ${\mathit{H}}_{\mathbf{1}}$ are shown as follows:

$${\mathit{F}}_1=\left[\begin{array}{cc}-{\displaystyle \frac{1}{\tau }}·{\mathit{I}}_3& {\mathbf{0}}_{3\times 3N}\\ {\mathbf{0}}_{3N\times 3}& {\mathbf{0}}_{3N\times 3N}\end{array}\right],$$ (8)

$${\mathit{H}}_{\mathbf{1}}=\left[{\mathbf{1}}_N\otimes {\mathit{I}}_3,{\mathit{I}}_{3N}\right],$$ (9)

where ${\mathbf{1}}_N$ is N-dimensional all-ones column vector, $\mathit{I}$ represents identity matrix with appropriate dimension, ⊗ in Equation (9) is the Kronecker product. In following sections, ⊗ denotes quaternion multiplication. After discretizing the state-space model (6), the estimate of the true angular rate can be obtained using a discrete Kalman filter:

$${\mathit{P}}_{k|k-1}={\mathit{F}}_{k,k-1}{\mathit{P}}_{k-1}{\mathit{F}}_{k,k-1}^T+{\mathit{Q}}_{k-1},$$ (10)

$${\mathit{K}}_k={\mathit{P}}_{k|k-1}{\mathit{H}}_k^T{({\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^T+{\mathit{R}}_k)}^{-1},$$ (11)

$${\mathit{P}}_k=(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k){\mathit{P}}_{k|k-1}{(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k)}^T+{\mathit{K}}_k{\mathit{R}}_k{\mathit{K}}_k^T,$$ (12)

$${\widehat{\mathit{X}}}_k={\mathit{F}}_{k,k-1}{\widehat{\mathit{X}}}_{k-1}+{\mathit{K}}_k({\mathit{Z}}_k-{\mathit{H}}_k{\mathit{F}}_{k,k-1}{\widehat{\mathit{X}}}_{k-1}).$$ (13)

3. Multi-Sensor Fusion Attitude Estimation Algorithm

This section presents the design of an Error State Kalman Filter (ESKF), which integrates data from the inertial array and magnetic sensor to estimate attitude quaternions. This primarily covers the principle of strap-down attitude estimation, multi-sensor fusion, and optimal attitude quaternion Kalman filter. The overall framework diagram is shown in Figure 1.

Figure 1.

Framework of the proposed algorithm.

3.1. Strap-Down Attitude Resolution

Select right-front-up as the body coordinate system b ($X_b\text{-}{Y}_b\text{-}{Z}_b$), and north-east-up as the geographic coordinate system n ($X_n\text{-}{Y}_n\text{-}{Z}_n$), which also serves as the navigation coordinate system. The conversion between attitude angles (yaw, pitch, roll) and quaternions is very common in the three-dimensional rotation calculation. The quaternion can be used to represent the attitude and offer high computational efficiency.

The real quaternion of the carrier is denoted as $q={[q_0,q_1,q_2,q_3]}^T$. The attitude matrix ${\mathit{C}}_b^n$ represents the transformation from the body coordinate b to the navigation coordinate n:

$${\mathit{C}}_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right],$$ (14)

The derivative of the quaternion $\mathit{q}$ can be expressed as:

$$\dot{\mathit{q}}={\displaystyle \frac{1}{2}}\mathit{q}\otimes \left[\begin{array}{c}0\\ \mathit{\omega }\end{array}\right].$$ (15)

where $\mathit{\omega }={[{\mathit{\omega }}_x,{\mathit{\omega }}_y,{\mathit{\omega }}_z]}^T$. ${\mathit{C}}_b^n$ can be obtained by solving the quaternion differential equation, and the attitude angles can then be calculated from ${\mathit{C}}_b^n$:

$$\left\{\begin{array}{c}\theta =\arcsin \left(C_b^n(3,2)\right)\hfill \\ \gamma =-\arctan \left(C_b^n(3,1)/C_b^n(3,3)\right)\hfill \\ \psi =\arctan \left(C_b^n(1,2)/C_b^n(2,2)\right)\hfill \end{array}\right.,$$ (16)

where $\theta$ is pitch angle, $\gamma$ is roll angle, and $\psi$ is yaw angle.

3.2. Error State Kalman Filter Algorithm Design

The Error State Kalman Filter (ESKF) decomposes the system state into a nominal state and an error state, performing linearization estimation only on the small-magnitude error state, thereby improving filtering accuracy and numerical stability. Its advantage lies in avoiding the high-order truncation errors associated with linearization in the traditional extended Kalman filter, making it particularly suitable for applications such as inertial navigation and multisensor fusion.

The system state consists of the nominal state and the error state. The nominal state is as follows:

$${\mathit{x}}_{\mathrm{nom}}=\left\{{\widehat{\mathit{q}}}_{nb},{\widehat{\mathit{b}}}_g\right\},$$ (17)

where ${\widehat{\mathit{q}}}_{nb}$ is the nominal attitude quaternion, ${\widehat{\mathit{b}}}_g$ is the estimated zero bias of the fused angular rate. The error state is represented as 6-dimensional vector, all defined in the local tangent space, and satisfy the linearity assumption:

$$\delta \mathit{x}={\left[\delta {\mathit{\theta }}^T\delta {\mathit{b}}_g^T\right]}^T\in {\mathbb{R}}^6,$$ (18)

where $\delta \mathit{\theta }$ is attitude rotation error, $\delta {\mathit{b}}_g$ is bias error of fused gyro.

The relationship between the actual attitude and the nominal attitude is defined as:

$${\mathit{q}}_{nb}={\widehat{\mathit{q}}}_{nb}\otimes \delta \mathit{q},\delta \mathit{q}={\mathrm{Exp}}_q\left(\delta \mathit{\theta }\right)\approx \left[\begin{array}{c}1\\ {\displaystyle \frac{1}{2}}\delta \mathit{\theta }\end{array}\right],$$ (19)

where ${\mathit{q}}_{nb}$ represents the attitude matrix ${\mathit{C}}_b^n$, $\delta \mathit{q}$ is the error quaternion formed by the rotation vector $\delta \mathit{\theta }$, ${\mathrm{Exp}}_q(·)$ is the SO(3) exponential map, which transforms a 3-dimensional rotation vector into the corresponding quaternion, ⊗ denotes quaternion multiplication.

The fused gyro measurement model satisfies the following relationship:

$$\tilde{\mathit{\omega }}=\mathit{\omega }+{\mathit{b}}_g+{\mathit{n}}_g,$$ (20)

where $\tilde{\mathit{\omega }}$ is the fused angular rate, ${\mathit{n}}_g$ is the Gaussian white noise.

Nominal angular rate $\widehat{\mathit{\omega }}=\tilde{\mathit{\omega }}-{\widehat{\mathit{b}}}_g$, substituting the error definition yields the angular rate error:

$$\mathit{\omega }-\widehat{\mathit{\omega }}=-\delta {\mathit{b}}_g-{\mathit{n}}_g.$$ (21)

The real and nominal quaternion dynamics are as follows:

$$\dot{\mathit{q}}={\displaystyle \frac{1}{2}}\mathit{q}\otimes \left[\begin{array}{c}0\\ \mathit{\omega }\end{array}\right],\dot{\widehat{\mathit{q}}}={\displaystyle \frac{1}{2}}\widehat{\mathit{q}}\otimes \left[\begin{array}{c}0\\ \widehat{\mathit{\omega }}\end{array}\right].$$ (22)

Take the derivative of $\mathit{q}=\widehat{\mathit{q}}\otimes \delta \mathit{q}$ and multiply both sides by ${\widehat{\mathit{q}}}^{-1}$:

$$\delta \dot{\mathit{q}}={\displaystyle \frac{1}{2}}(\delta \mathit{q}\otimes \mathit{\omega }-\widehat{\mathit{\omega }}\otimes \delta \mathit{q}).$$ (23)

Taking the small error approximation and neglecting second order terms, we can derive the differential equation for the attitude error:

$$\delta \dot{\mathit{\theta }}=-[\widehat{\mathit{\omega }}\times ]\delta \mathit{\theta }-\delta {\mathit{b}}_g-{\mathit{n}}_g,$$ (24)

where $\delta {\mathit{b}}_g={\mathit{b}}_g-{\widehat{\mathit{b}}}_g$, and $\dot{\delta {\mathit{b}}_g}={\mathit{n}}_{bg}$, ${\mathit{n}}_{bg}$ is the Gaussian white noise, $[\widehat{\mathit{\omega }}\times ]$ is the antisymmetric matrix corresponding to true angular rate.

Thus, the error state system equation is:

$$\left\{\begin{array}{c}{\mathit{X}}_2\left(t\right)=\delta \mathit{x}={\left[\delta {\mathit{\theta }}^T\delta {\mathit{b}}_g^T\right]}^T\hfill \\ {\dot{\mathit{X}}}_2\left(t\right)={\mathit{F}}_2·{\mathit{X}}_2\left(t\right)+{\mathit{G}}_2·{\mathit{W}}_2\left(t\right)\hfill \end{array}\right.,$$ (25)

where system process noise ${\mathit{W}}_2={[{\mathit{n}}_g^T,{\mathit{n}}_{bg}^T]}^T$, ${\mathit{I}}_{3\times 3}$ and ${\mathbf{0}}_{3\times 3}$ denote identity matrix and full zero matrix, the coefficient matrices are written as:

$${\mathit{F}}_2=\left[\begin{array}{cc}-\left[\widehat{\mathit{\omega }}\times \right]& -{\mathit{I}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right],{\mathit{G}}_2=\left[\begin{array}{cc}-{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}\end{array}\right].$$ (26)

Based on the output signals from the accelerometer and magnetometer, there are two methods for deriving the measurement equation. The core idea is to use long-term stable reference values to correct the cumulative error in attitude calculations. One approach is to transform the attitude estimation problem into a nonlinear least-squares problem and solve it using an iterative method. Another approach is to directly extract the output to establish the measurement equation. The different types of sensors in the latter method are decoupled, making it easier to handle potential disturbances.

This part utilizes a direct vector observation scheme, employing normalized acceleration and geomagnetic measurements directly as filter observations to avoid accuracy losses in intermediate steps. At the same time, a sequential update strategy is employed to achieve decoupled calibration of the two types of sensors.

Define the unit reference vectors in the navigation coordinate n as ${\mathit{r}}_a^n$ and ${\mathit{r}}_m^n$; ${\mathit{r}}_a^n$ is the normalized reference direction of the accelerometer, and ${\mathit{r}}_m^n$ is the normalized geomagnetic reference vector.

The outputs of the accelerometer and magnetometer, after normalization, can be expressed as

$${\mathit{z}}_a={\displaystyle \frac{{}^b\mathit{a}^m}{\parallel {}^b\mathit{a}^m\parallel }},$$ (27)

$${\mathit{z}}_m={\displaystyle \frac{{}^b\mathit{m}^m}{\parallel {}^b\mathit{m}^m\parallel }}.$$ (28)

Under the nominal attitude, the predicted observation in body coordinate b:

$${\mathit{h}}_a\left(\widehat{\mathit{q}}\right)={\mathit{C}}_b^n{\left(\widehat{\mathit{q}}\right)}^T{\mathit{r}}_a^n,$$ (29)

$${\mathit{h}}_m\left(\widehat{\mathit{q}}\right)={\mathit{C}}_b^n{\left(\widehat{\mathit{q}}\right)}^T{\mathit{r}}_m^n.$$ (30)

The attitude matrix satisfies:

$${\mathit{C}}_b^n={\widehat{\mathit{C}}}_b^n·\mathrm{Exp}\left({\left[\delta \mathit{\theta }\right]}_{\times }\right)\approx {\widehat{\mathit{C}}}_b^n(\mathit{I}+[\delta \mathit{\theta }\times ]).$$ (31)

Thus, the observation residual can be derived as:

$${\mathit{r}}_a={\mathit{z}}_a-{\mathit{h}}_a\left(\widehat{\mathit{q}}\right)\approx [{\mathit{h}}_a\left(\widehat{\mathit{q}}\right)\times ]\delta \mathit{\theta }+{\mathit{v}}_a,$$ (32)

$${\mathit{r}}_m={\mathit{z}}_m-{\mathit{h}}_m\left(\widehat{\mathit{q}}\right)\approx [{\mathit{h}}_m\left(\widehat{\mathit{q}}\right)\times ]\delta \mathit{\theta }+{\mathit{v}}_m,$$ (33)

where $\mathit{v}$ represents Gaussian white noise.

The Jacobian matrix can be written as:

$${\mathit{H}}_a=\left[[{\mathit{h}}_a\left(\tilde{\mathit{q}}\right)\times ]{\mathbf{0}}_{3\times 3}\right],$$ (34)

$${\mathit{H}}_m=\left[[{\mathit{h}}_m\left(\tilde{\mathit{q}}\right)\times ]{\mathbf{0}}_{3\times 3}\right].$$ (35)

For each valid observation, calculate the gain and covariance using the Kalman filter framework:

$$\mathit{K}={\mathit{P}}^{-}{\mathit{H}}^T{(\mathit{H}{\mathit{P}}^{-}{\mathit{H}}^T+\mathit{R})}^{-1},$$ (36)

$$\mathit{P}=(\mathit{I}-\mathit{KH}){\mathit{P}}^{-}{(\mathit{I}-\mathit{KH})}^T+{\mathit{KRK}}^T,$$ (37)

$${\widehat{\mathit{X}}}_2=\mathit{K}·{\mathit{r}}_i.$$ (38)

After obtaining the error attitude estimate $\delta \widehat{\mathit{\theta }}$, construct the correction quaternion:

$$\delta \widehat{\mathit{q}}={\mathrm{Exp}}_q\left(\delta \widehat{\mathit{\theta }}\right)\approx {\left[\begin{array}{c}1,{\displaystyle \frac{1}{2}}\delta \widehat{\mathit{\theta }}\end{array}\right]}^T,$$ (39)

$${\widehat{\mathit{q}}}^{+}={\widehat{\mathit{q}}}^{-}\otimes \delta \widehat{\mathit{q}},$$ (40)

$${\widehat{\mathit{b}}}_{\mathit{g}}^{+}={\widehat{\mathit{b}}}_{\mathit{g}}^{-}+\delta {\widehat{\mathit{b}}}_{\mathit{g}}.$$ (41)

After inserting the error into the nominal state, reset the error state and covariance. The above update employs a sequential update mechanism, in which the accelerometer observation update is performed first, followed by the magnetometer observation update. Two independent 3-dimensional Kalman updates are equivalent to a single stacked 6-dimensional joint update; however, sequential updates can better handle disturbances from different sensors.

For typical sources of disturbance such as dynamic acceleration and local magnetic anomalies, smoothing and weighting are achieved by adaptively amplifying the covariance of the measurement noise. The metrics used include length deviation and direction residual.

$$d_{\mathrm{norm}}=\left|{\displaystyle \frac{\parallel {\mathit{a}}_m\parallel }{{\mathit{a}}_{\mathrm{ref}}}}-1\right|,$$ (42)

$$d_{\mathrm{angle}}=\arccos \left({\mathit{z}}_a^T{\mathit{h}}_a\left(\widehat{\mathit{q}}\right)\right),$$ (43)

where $d_{\mathrm{norm}}$ is the length deviation of accelerometer, ${\mathit{a}}_{\mathrm{ref}}$ is the reference gravity, $d_{\mathrm{angle}}$ represents the angle between the direction of the actual acceleration and the direction of the predicted gravity based on attitude. Set soft threshold $T_{\mathrm{norm}}$, $T_{\mathrm{angle}}$ and hard threshold $H_{\mathrm{norm}}$, $H_{\mathrm{angle}}$ for the two metrics, and proceed as follows in two steps:

• If $d_{\mathrm{norm}}>H_{\mathrm{norm}}$ or $d_{\mathrm{angle}}>H_{\mathrm{angle}}$, the current observation is deemed invalid, and the filter update is skipped;

• When the hard threshold is not triggered, the greater the deviation exceeds the soft threshold, the higher the noise gain: $s=\max \left(1,{\displaystyle \frac{d_{\mathrm{norm}}}{T_{\mathrm{norm}}}}\right)·\max \left(1,{\displaystyle \frac{d_{\mathrm{angle}}}{T_{\mathrm{angle}}}}\right)$;

• The final adaptive measurement noise covariance is: ${\mathit{R}}_2=s·{\mathit{R}}_0$, ${\mathit{R}}_0$ is the initial value.

The calculation method for the relevant metrics of magnetometer are the same.

4. Simulation and Experiment

This section presents the simulation verification and experiments for the proposed algorithm. Simulations were conducted to verify the effectiveness of the two-stage filter algorithm. Experiments were conducted to verify the actual performance in real applications.

4.1. Simulation Results and Analysis

To comprehensively evaluate the effectiveness of the proposed algorithm, this study constructed a navigation simulation environment. In the simulation, there are four three-axis gyroscopes, one accelerometer, and one magnetometer. Furthermore, the proposed algorithm is applicable to any reasonable number of gyroscopes.

In array simulation, angle random walk is $0.08°/\sqrt{h}$, and rate random walk is $600.0°/h^{1.5}$. The performance improvement of the virtual gyroscope is verified under static conditions. The simulation results of the gyro array under static conditions are shown in Figure 2 and Figure 3.

Figure 2.

Comparison of gyro array fusion results with original angular rate.

Figure 3.

Allan variance plot under the static condition.

Figure 2 indicates that the noise level of the fused angular rate is lower than that of the raw measurements, demonstrating that the Kalman filter can effectively suppress random noise. In the Allan variance plot, the curve of the fused angular rate exhibits a smaller amplitude, further confirming the reduced noise level. The ARW is reduced to $0.02°/\sqrt{h}$, and the RRW is reduced to $149.83°/h^{1.5}$. Furthermore, the simulation results demonstrate that employing the redundant array can effectively mitigate subsequent error accumulation.

For the second-stage attitude estimation algorithm, simulation in dynamic scenarios was used to verify the algorithm’s effectiveness. Considering the motion characteristics of the actual vehicle during the cruise phase, we designed a composite motion trajectory with low dynamics and a long period. In addition, to verify the robustness of the algorithm against external disturbances, dynamic acceleration and magnetic disturbance were introduced.

The simulation results of the ESKF algorithm are shown in Figure 4 and Figure 5. Single MIMU refers to the strapdown attitude solution from a single gyroscope. The fused gyro uses the fused angular rate signal. Acc-Mag represents the attitude estimate obtained through the iterative solution method using the accelerometer and the magnetometer [22]. The proposed integrated attitude estimation algorithm achieves the highest accuracy. In contrast, relying solely on gyroscope measurements leads to rapid divergence of the attitude error. Meanwhile, fusing the output signals of the four gyroscopes improves the measurement accuracy of the angular rate. However, the accumulation of errors in strapdown attitude computation still exists.

Figure 4.

Comparison of the reference value and attitude estimation results using different methods in simulation.

Figure 5.

Comparison of attitude estimation errors.

Additionally, the attitude of the carrier can be determined using the measurements from the accelerometer and the magnetometer. However, this method is highly susceptible to disturbance. As shown in Figure 4, both dynamic acceleration and magnetic field disturbances can lead to severe attitude errors. Conversely, the ESKF can adaptively adjust the noise covariance in the presence of disturbances, and the sequential update method decouples the accelerometer and magnetometer. The accelerometer primarily constrains roll and pitch errors, while the magnetometer mainly constrains the heading angle error.

The integrated algorithm combines the advantages of these two approaches. By using gyro integration results as a basis and introducing the invariant external references, it resolves the problem of error accumulation and yields high-precision attitude angles. The adaptive noise covariance used also helps suppress the effects of external disturbances.

4.2. Experimental Results and Analysis

To further validate the effectiveness of the proposed algorithm in actual environments, field tests were designed. The test platform is shown in Figure 6.

Figure 6.

The designed multi-sensor testing MIMU and the experimental platform for field tests.

The integrated navigation system consists of a micro-inertial measurement unit and a magnetometer and can measure navigation data along three axes. The micro-inertial measurement unit includes eight gyroscopes and one accelerometer. The gyroscopes are arranged in a planar array. A high-precision fiber-optic inertial measurement unit (FOG-IMU) was used as the reference system. The experiment is divided into two parts: gyro array fusion and attitude estimation.

4.2.1. Experimental Results for Gyroscope Array Fusion

The test platform is equipped with a MEMS gyro array consisting of eight homogeneous three-axis gyroscopes, capable of outputting three-axis measurement signals. In Section 2, the Kalman filter algorithm for fusing redundant measurements is designed to serve as the first-stage filter in the proposed integrated navigation algorithm. The angular rate fusion results are shown in Figure 7, Figure 8 and Figure 9.

Figure 7.

The original angular rate and the fused angular rate along the X-axis.

Figure 8.

The original angular rate and the fused angular rate along the Y-axis.

Figure 9.

The original angular rate and the fused angular rate along the Z-axis.

The test results show that the designed Kalman filter algorithm can effectively improve the accuracy of angular rate measurements. Under experimental conditions, the impact of random noise on attitude estimation was reduced. The first-stage filtering removes random noise from the angular rate signal before it is used. As in the simulation, the accuracy of the solution obtained using a fused rate is much higher than that obtained using a single gyroscope.

Array fusion is the first-stage filtering process designed to provide more accurate angular rate for the second-stage attitude estimation. After completing the fusion of redundant gyroscopes, the strapdown attitude calculation can be performed to obtain a preliminary estimate of the attitude quaternion.

4.2.2. Experimental Results for Attitude Estimation

The navigation simulation validated the feasibility of the proposed algorithm. To further verify the performance of the algorithm in a real-world environment, two sets of field tests were designed. The experiments adopt the TY-MIGA-100A integrated navigation system. This system supports static and dynamic initial alignment and compensates for installation errors via external parameters. It can also perform magnetometer calibration and compensate for the hard-iron and soft-iron effects. Calibration was completed using the manufacturer-provided software, which is not elaborated here since it is beyond the scope of this work.

The parameters used in the navigation experiments are shown in Table 1. Set the sampling frequency to 100 Hz. The filter is initialized via the static initialization method, where the initial attitude is calculated using accelerometer and magnetometer measurements. Part of the noise parameters are taken from the device datasheet, while the rest are obtained by performing Allan Variance analysis on measured static-state data. The hard threshold indicates that data are invalid when sensor deviations exceed the threshold. The soft threshold means deviations are tolerable and only require adjustment of the noise covariance.

Table 1.

Parameter setting.

Parameter	Value
Sampling Frequency	100 Hz
Initial Attitude Error Std	5°
Initial Bias Error Std	$1°/\mathrm{s}$
Acc Noise Std	0.05
Acc Bias Stability	0.01 g
Gyro Noise Std	$0.2°/\sqrt{h}$
Gyro Bias Stability	$10°/\mathrm{h}$
Mag Noise Std	0.04
Correlation Time Constant	1000 s
Acc Soft/Hard Norm Threshold	0.05/0.25
Mag Soft/Hard Norm Threshold	0.08/0.30
Acc Soft/Hard Angle Threshold	$10°/60°$
Mag Soft/Hard Angle Threshold	$12°/45°$

Various methods are used to estimate attitude, including strapdown inertial integration, estimation using accelerometer and magnetometer (Acc-Mag), and the proposed error state Kalman filter (ESKF). The results are shown in Figure 10, Figure 11, Figure 12, Figure 13, Figure 14, Figure 15, Figure 16, Figure 17, Figure 18, Figure 19, Figure 20 and Figure 21. Table 2 and Table 3 present the Root Mean Square Error (RMSE) and Mean Absolute Error (MAE) metrics for the two tests.

$$\mathrm{RMSE}=\sqrt{{\displaystyle \frac{1}{n}}\sum _{i=1}^n{(Eule{r}_E-Eule{r}_R)}_k^2}$$ (44)

$$\mathrm{MAE}={\displaystyle \frac{1}{n}}{\displaystyle \sum _{k=1}^n}\left|{({\mathrm{Euler}}_E-{\mathrm{Euler}}_R)}_k\right|$$ (45)

where ${\mathrm{Euler}}_E$ is the estimated value, ${\mathrm{Euler}}_R$ is the reference value.

Figure 10.

The reference value and strapdown attitude estimation results for Test 1.

Figure 11.

The reference value and attitude estimation results determined using the accelerometer and magnetometer in Test 1.

Figure 12.

The reference value and attitude estimation results obtained using error state Kalman filter in Test 1.

Figure 13.

Comparison of attitude estimation errors determined via different methods in Test 1.

Figure 14.

The reference value and strapdown attitude estimation results for Test 2.

Figure 15.

The reference value and attitude estimation results determined using the accelerometer and magnetometer in Test 2.

Figure 16.

The reference value and attitude estimation results obtained using error state Kalman filter in Test 2.

Figure 17.

Comparison of attitude errors determined via different methods in Test 2.

Figure 18.

The observation residual angle in Test 2.

Figure 19.

The norm deviation percentage in Test 2.

Figure 20.

The noise covariance scaling factor in Test 2.

Figure 21.

The observation acceptance flag in Test 2.

Table 2.

Comparison of statistical metrics for Test 1.

Metrics	Euler Angle	Strapdown	Acc-Mag	ESKF
RMSE (deg)	Roll	2.4036	1.8734	1.0353
	Pitch	0.7692	1.4077	0.4587
	Yaw	4.9495	2.7002	1.5927
MAE (deg)	Roll	2.0612	1.2288	0.6056
	Pitch	0.4748	0.9226	0.3021
	Yaw	4.3112	1.9626	1.0787

Table 3.

Comparison of statistical metrics for Test 2.

Metrics	Euler Angle	Strapdown	Acc-Mag	ESKF
RMSE (deg)	Roll	1.6566	1.8786	0.7399
	Pitch	1.5415	2.1441	1.0752
	Yaw	3.7437	3.4635	1.6579
MAE (deg)	Roll	1.2369	1.3331	0.3949
	Pitch	1.2517	1.5172	0.6306
	Yaw	3.2582	2.4932	1.0865

The attitude angle estimation results for Test 1 are shown in Figure 10, Figure 11, Figure 12 and Figure 13. As can be seen from the results, all three methods are capable of estimating the attitude of carrier.

The results of strapdown inertial integration are shown in Figure 10. The estimated attitude angles are relatively smooth, but errors accumulate over time. In Figure 11, there is no error accumulation when determining attitude using the accelerometer and magnetometer. However, due to the external disturbances, high frequency fluctuations are present in the results. Figure 12 shows that the proposed error state Kalman filter can achieve accurate estimation of attitude angles. Furthermore, the estimation results do not exhibit the error accumulation shown in Figure 10, and the issue of low accuracy shown in Figure 11 has been resolved. The error comparison in Figure 13 shows that the ESKF achieves the highest accuracy.

To further validate the performance of the ESKF algorithm, a different vehicle route was selected for test 2. The experimental results are shown in Figure 14, Figure 15, Figure 16 and Figure 17.

From Figure 14, Figure 15, Figure 16 and Figure 17, the experimental results lead to the same conclusion in Test 2. The proposed error state Kalman filter combines the advantages of both methods, addresses the issues of error accumulation and high-frequency disturbances, and yields the optimal attitude estimation. As shown in Table 2 and Table 3, the RMSE and MAE values for the ESKF are significantly lower than those of other comparison methods.

To verify the effectiveness of the disturbance handling mechanism used in the proposed algorithm, a more detailed analysis of the disturbance is provided using Test 2 as an example. The results are shown in Figure 18, Figure 19, Figure 20 and Figure 21.

Figure 18 and Figure 19 show observation residual angle and norm deviation percentage in the experiment. Observation residual angle and norm deviation represent the errors between the actual measurements from component sensors and the theoretical calculations. It shows that a strong acceleration disturbance exists in the experiment, while the overall magnetic field deviation is low, indicating that the magnetic environment is relatively stable. Figure 20 represents the noise covariance scaling factor. When observation anomalies are detected, the observation noise covariance is inflated to down-weight the anomalous measurements, with a larger scaling factor reflecting lower reliability of the corresponding sensor data. Figure 21 represents the observation acceptance flag, which indicates that the mechanism ensures system stability and robustness by rejecting anomalous observations. The analysis results show that the disturbance handling mechanism can improve the robustness of proposed algorithm.

5. Conclusions

In this work, a multi-sensor fusion attitude estimation algorithm is proposed. The integrated system includes a redundant array of homogeneous MEMS gyroscopes, as well as an accelerometer and a magnetometer. A two-stage data fusion algorithm was employed. The first-stage Kalman filter is used to fuse angular rate signals from multiple gyroscopes. In the second stage, the error state Kalman filter receives measurements from the accelerometer and magnetometer, updates the error state, and produces the final attitude estimate after correction. Simulation and experimental results show that the gyro array can effectively improve the measurement accuracy of angular rate and reduce the impact of random noise. Meanwhile, the proposed error state Kalman filter can obtain the optimal attitude estimate. The error profile of ESKF is much smoother and with lower values. Compared to the RMSE of strapdown inertial solution, Roll was reduced by 55.34%, Pitch by 30.25%, and Yaw by 55.71%. The proposed method offers an effective approach to low-cost, high-precision attitude estimation. In the future, further research can be conducted to expand the number of array sensors, improve the noise adaptive mechanism, and extend the application scenarios, while the current experimental validation is limited, a more comprehensive comparison with other advanced algorithms is also identified as a primary direction in our future work.

Author Contributions

Conceptualization, L.X. and J.L.; methodology, L.X. and J.L.; software, J.L.; formal analysis, B.Y. and X.W.; data curation, B.Y. and X.W.; writing—original draft preparation, J.L.; writing—review and editing, L.X. and G.C.; visualization, B.Y. and X.W.; supervision, L.X. and G.C.; funding acquisition, G.C. All authors have read and agreed to the published version of the manuscript.

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

The data presented in this study are available on request from the corresponding author. (The data are not publicly available due to privacy.)

Conflicts of Interest

The authors declare no conflicts of interest.

Funding Statement

This research was funded by National Natural Science Foundation of China (grant number 62473374, 62403487, U2441243) and the Natural Science Foundation of Shaanxi Province (grant number 2025JC-YBMS-711).