A Sequential Adaptive Linear Kalman Filter Based on the Geophysical Field for Robust MARG Attitude Estimation

Abstract

In magnetometer, accelerometer, and rate gyroscope (MARG) attitude and heading reference systems, accelerometers and magnetometers are susceptible to external acceleration and soft/hard magnetic anomalies, which reduce the attitude estimation accuracy. To address this problem, a sequential adaptive Kalman filter algorithm based on the geophysical field is proposed for anti-interference MARG attitude estimation. By establishing the linear system model based on the gravitational field and geomagnetic field, the singularity and coupling in other system models are avoided. Additionally, the sequential Sage–Husa adaptive strategy is employed to estimate the measurement noise parameters in real time by a specific force and magnetic vector, which suppresses the impact of external acceleration and the soft/hard magnetic anomalies. To verify the effectiveness and advancement of the proposed algorithm, a series of anti-interference experiments were designed. Experimental results show that, compared with the geophysical-field-based Kalman filter algorithm without an adaptive strategy, the proposed improved algorithm reduces the yaw maximum error by over 94% and inclination maximum error by over 21%, which improves the MARG attitude estimation robustness and makes this algorithm superior to the existing three adaptive strategies and two algorithms.

1. Introduction

With the rapid advancement of Micro-Electro-Mechanical System (MEMS) technology, the attitude and heading reference system (AHRS) has been widely used in various fields such as indoor positioning [1], mobile robots [2], wearable devices [3], autonomous driving [4], unmanned aerial vehicles [5], resource exploration [6], virtual reality [7], attitude capture [8], and stone and earthen materials testing [9]. Therefore, how to obtain accurate attitude information by MEMS-AHRS is a current research hotspot in applied sciences.

At present, MEMS-AHRS’s accuracy remains limited under dynamic conditions [10]. Although MEMS gyroscopes offer benefits such as low cost, low power consumption, and small size, the significant zero drift of MEMS gyroscopes can lead to Euler angle divergence when used alone [11]. To address this difficulty, magnetometer, accelerometer, and rate gyro (MARG) sensors are commonly used to improve the attitude estimation accuracy based on the specific geophysical field, viz., the gravitational field and the geomagnetic field [12].

Currently, numerous data fusion algorithms are applied for MARG attitude estimation, such as complementary filters [13], Kalman filters [14], and neural networks [15]. The complementary filter is simple but relies on manually tuned parameters, making it hard to adapt to complex conditions [16]. Neural networks have strong fitting ability but require large datasets and lack interpretability, limiting their use on resource-constrained platforms [17]. In contrast, the Kalman filter provides optimal estimation based on system models and noise statistics, offering both accuracy and stability. From the perspective of selecting a filter state vector, the general options are algorithms based on attitude representation parameters, such as the Euler angle [18], rotation matrix [19], or quaternion [20]. N.Y. Ko et al. [18] proposed a Euler angle-based extended Kalman filter algorithm, which modifies the gyroscope-predicted Euler angle using the Euler angle measurement calculated by the specific force and magnetic vector. However, Euler angle-based algorithms involve a lot of inverse trigonometric functions during each attitude update and are prone to Gimbal Lock [21]. G. Shaaban et al. [19] proposed a rotation-matrix-based invariant extended Kalman filter algorithm, which derives a rotation matrix propagation equation based on the angular rate measured by the gyroscopes and fuses the specific force and magnetic vector measurement through non-linear transformation. However, rotation-matrix-based algorithms encounter high-dimensional and multi-constraint challenges, resulting in low computational efficiency [22]. Unlike the Euler angle and rotation matrix, quaternion-based algorithms do not suffer from these problems. X. Xu et al. [20] proposed a quaternion-based double extended Kalman filter algorithm, which decomposes the quaternion into the inclination quaternion and the yaw quaternion and corrects inclination and yaw using an extended Kalman filter. In contrast, quaternion-based algorithms require decoupling of the influence of magnetic interference on inclination estimation, which is caused by the presence of magnetic inclination. Therefore, S. Zhang et al. [23] proposed a geophysical-field-based Kalman filter (GKF) algorithm, which avoids using attitude representation parameters as the filter state vector. Instead, the algorithm estimates the specific force and magnetic vector information in the body frame based on the propagation equations of the gravitational field and geomagnetic field. Finally, the attitude information is calculated from the estimated specific force and magnetic vector.

In addition, the external acceleration [24] during vehicle movement and the soft/hard magnetic anomalies [12] caused by external factors such as metal and electronic equipment can disturb the attitude information obtained from MARG sensors. Hence, adaptive algorithms are needed to suppress the impact of external acceleration and magnetic anomalies on inclination and yaw estimation, respectively [25].

To solve the above problem, many scholars have carried out a lot of research and improvement on adaptive strategies. Y. Sun et al. [26] proposed a decoupled orientation estimation approach (DOEA) based on the threshold-based adaptive (TA) strategy, which judges the reliability of accelerometers by the modulus value of the accelerometer-sensed gravity field vector. Similarly, X. Wang et al. [27] proposed an adaptive robust error-state Kalman filter algorithm to enhance yaw estimation in complex magnetic disturbance environments, which dynamically adjusts the magnetic vector measurement noise by calculating the changes in magnetic field intensity and magnetic inclination. Nonetheless, Y. Xu et al. [28] believe that TA strategy may lead to misjudgment, which cannot ensure the reliability of measurements. The derivative of the output vector of the accelerometers and magnetometers should be regarded as an additional judgment criterion. A. Lotfy et al. [29] used the Chi-square test adaptive (CA) Kalman filter algorithm to monitor the measurement variance using the measurement information and the mean square error matrix. However, the determination of the detection threshold in CA is challenging due to the principle of binary measurement anomaly judgment, which may lead to false alarms or a reduction in the probability of detecting measurement anomalies. S. Sarkka et al. [30] proposed the Variational Bayesian adaptive (VBA) Kalman filter algorithm, which estimates the inaccurate and slowly varying measurement noise covariance matrix by choosing an appropriate conjugate prior to distribution. A. Makni et al. [31] used the Sage–Husa adaptive filter algorithm to estimate the measured noise parameters of the system in real time through the output of the accelerometers. However, the Sage–Husa adaptive filter is sensitive to initial value selection. The measurement noise covariance matrix easily loses positive quality, which can lead to filter divergence. As a result, X. Lin et al. [32] proposed a robust error-state Kalman filter (RESKF) algorithm based on the improved sequential adaptive (SA) strategy for MARG attitude estimation, which uses a sequential structure to reduce adaptive parameters and impose upper and lower boundary limits to ensure that the measure noise covariance matrix remains within a reasonable range and achieve fault isolation to rapidly reduce the measurement credibility. However, the noise covariance of a specific force and magnetic vector in the RESKF algorithm is not propagated to the Euler angle measurement, which impacts the optimal estimation of the filter. In addition, MARG adaptive attitude estimation can also be achieved by updating the magnetometer calibration parameters. H. Ke et al. [33] proposed a gyroscope-aided magnetometer calibration algorithm, which updates the calibration parameters based on the consistency of spatial rotation perception between the gyroscopes and the magnetometers. Z. Wu et al. [34] proposed an adaptive anti-disturbance algorithm, which estimates the magnetometer zero bias and the rate of change in the magnetic field to dynamically calibrate the magnetometer parameters, thereby maintaining filter stability under long-term stable magnetic interference.

In this work, a sequential adaptive geophysical-field-based Kalman filter (SA-GKF) algorithm for MARG attitude estimation is proposed. The SA-GKF algorithm estimates the specific force and magnetic vector based on the linear system model of the specific geophysical field and uses the sequential Sage–Husa adaptive strategy to estimate the measurement noise parameters in real time, which suppresses the maximum error of yaw estimation by more than 94% and inclination estimation by more than 21%. Furthermore, compared with three adaptive strategies and two algorithms, the proposed algorithm shows superior accuracy, confirming its advancement in attitude estimation under challenging conditions.

This paper is organized as follows: In Section 2, the preliminaries of the rotation matrix calculation, the inclination calculation, and the yaw calculation are reviewed. In Section 3, the sequential adaptive geophysical-field-based Kalman filter (SA-GKF) algorithm for MARG attitude estimation is introduced. In Section 4, a set of experiments are designed to verify the effectiveness and advancement of the SA-GKF algorithm. In Section 5, the conclusions are presented.

2. Preliminaries

This section mainly reviews the fundamental principles of deriving the Euler angle from the geophysical field, mainly including the rotation matrix calculation, inclination calculation, and yaw calculation.

2.1. Rotation Matrix Calculation

The navigation frame {$n$} is defined as the “east-north-up” frame, and the body frame {$b$} is defined as the “right-front-up” frame. The rotation matrix ${\mathit{C}}_b^n$ stands for the attitude of frame {$b$} relative to frame {$n$}, which can be expressed by the Euler angle ${\mathit{A}}_k={\left[\begin{array}{ccc}{\theta }_k& {\gamma }_k& {\psi }_k\end{array}\right]}^T$ as follows:

$$\begin{array}{ll}{\mathit{C}}_b^n& =\left[\begin{array}{ccc}\cos {\psi }_k& -\sin {\psi }_k& 0\\ \sin {\psi }_k& \cos {\psi }_k& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\theta }_k& -\sin {\theta }_k\\ 0& \sin {\theta }_k& \cos {\theta }_k\end{array}\right]\left[\begin{array}{ccc}\cos {\gamma }_k& 0& \sin {\gamma }_k\\ 0& 1& 0\\ -\sin {\gamma }_k& 0& \cos {\gamma }_k\end{array}\right]\\ & =\left[\begin{array}{ccc}\cos {\psi }_k\cos {\gamma }_k-\sin {\psi }_k\sin {\theta }_k\sin {\gamma }_k& -\sin {\psi }_k\cos {\theta }_k& \cos {\psi }_k\sin {\gamma }_k+\sin {\psi }_k\sin {\theta }_k\cos {\gamma }_k\\ \sin {\psi }_k\cos {\gamma }_k+\cos {\psi }_k\sin {\theta }_k\sin {\gamma }_k& \cos {\psi }_k\cos {\theta }_k& \sin {\psi }_k\sin {\gamma }_k-\cos {\psi }_k\sin {\theta }_k\cos {\gamma }_k\\ -\cos {\theta }_k\sin {\gamma }_k& \sin {\theta }_k& \cos {\theta }_k\cos {\gamma }_k\end{array}\right]\end{array},$$

(1)

where ${\theta }_k$, ${\gamma }_k$, and ${\psi }_k$ are the pitch, roll, and yaw, respectively.

2.2. Inclination Calculation

The specific force in frame {$b$} at time $k$ is defined as ${\mathit{f}}_k^b={\left[\begin{array}{ccc}{f}_{x|k}^b& f_{y|k}^b& f_{z|k}^b\end{array}\right]}^T$; the specific force in frame {$n$} at time $k$ is defined as ${\mathit{f}}_k^n={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T$; and $g=1$ is the normalized local gravity acceleration. Therefore, ${\mathit{f}}_k^b$ can be expressed by ${\mathit{f}}_k^n$ and ${\mathit{C}}_b^n$:

$$\begin{array}{ll}{\mathit{f}}_k^b& ={\left({\mathit{C}}_b^n\right)}^T{\mathit{f}}_k^n\\ & =\left[\begin{array}{c}-\cos {\theta }_k\sin {\gamma }_k\\ \sin {\theta }_k\\ \cos {\theta }_k\cos {\gamma }_k\end{array}\right]\end{array}.$$

(2)

Further, the pitch and roll at time $k$ can be calculated from Equation (2) by ${\mathit{f}}_k^b$, i.e., [32]

$$\left\{\begin{array}{l}{\theta }_k=\arctan ({\displaystyle \frac{f_{y|k}^b}{\sqrt{(f_{x|k}^b{)}^2+(f_{z|k}^b{)}^2}}})\hfill \\ {\gamma }_k^{*}=\arctan (-{\displaystyle \frac{f_{x|k}^b}{f_{z|k}^b}})\hfill \end{array}\right..$$

(3)

In addition, ${\gamma }_k$ requires an additional true value judgment, as shown in

Table 1. True value judgment of roll.

${\mathit{\gamma }}_{\mathit{k}}^{*}$	${\mathit{f}}_{\mathit{z}|\mathit{k}}^{\mathit{b}}$	${\mathit{\gamma }}_{\mathit{k}}$
$+$	$+$	${\gamma }_k^{*}$
$-$	$+$	${\gamma }_k^{*}$
$+$	$-$	${\gamma }_k^{*}-{180}^{\circ }$
$-$	$-$	${\gamma }_k^{*}+{180}^{\circ }$

.

2.3. Yaw Calculation

The magnetic vector in frame {$b$} at time $k$ is defined as ${\mathit{m}}_k^b={\left[\begin{array}{ccc}{m}_{x|k}^b& m_{y|k}^b& m_{z|k}^b\end{array}\right]}^T$, and the magnetic vector in frame {$n$} is at time $k$ defined as ${\mathit{m}}_k^n={\left[\begin{array}{ccc}{m}_{x|k}^n& m_{y|k}^n& m_{z|k}^n\end{array}\right]}^T$. Therefore, ${\mathit{m}}_k^b$ can be expressed by ${\mathit{m}}_k^n$ and ${\mathit{C}}_b^n$, i.e.,

$$\begin{array}{ll}{\mathit{m}}_k^b& ={\left({\mathit{C}}_b^n\right)}^T{\mathit{m}}_k^n\\ & =\left[\begin{array}{c}-\cos {\theta }_k\sin {\gamma }_k\\ \sin {\theta }_k\\ \cos {\theta }_k\cos {\gamma }_k\end{array}\right]\end{array}.$$

(4)

In the same way, the yaw at time $k$ can be calculated from Equation (4) by ${\mathit{m}}_k^b$, i.e., [32]

$$\left\{\begin{array}{l}{m}_{x|k}^n=\cos {\gamma }_k{m}_{x|k}^b+\sin {\gamma }_k{m}_{z|k}^b\hfill \\ m_{y|k}^n=\sin {\gamma }_k\sin {\theta }_k{m}_{x|k}^b+\cos {\theta }_k{m}_{y|k}^b- \cos {\gamma }_k\sin {\theta }_k{m}_{z|k}^b\hfill \\ {\psi }_k^{*}=\arctan \left(-m_{x|k}^n/m_{y|k}^n\right)\hfill \end{array}\right..$$

(5)

In addition, ${\psi }_k$ requires an additional true value judgment, as shown in

Table 2. True value judgment of yaw.

${\mathit{m}}_{\mathit{x}|\mathit{k}}^{\mathit{n}}$	${\mathit{m}}_{\mathit{y}|\mathit{k}}^{\mathit{n}}$	${\mathit{\psi }}_{\mathit{k}}$
$-$	$+$	${\psi }_k^{*}$
$-$	$-$	${\psi }_k^{*}-{180}^{\circ }$
$+$	$+$	${\psi }_k^{*}$
$+$	$-$	${\psi }_k^{*}+{180}^{\circ }$

.

These equations will provide a theoretical basis for the design of the SA-GKF algorithm.

3. Sequential Adaptive Geophysical-Field-Based Kalman Filter Algorithm for Robust MARG Attitude Estimation

The purpose of this study is to suppress the effects of external acceleration and the soft/hard magnetic anomalies on the attitude estimation, for which the SA-GKF algorithm is proposed. Based on the principles outlined in Section 2, this section mainly describes the proposed SA-GKF algorithm for robust MARG attitude estimation.

In the SA-GKF algorithm, the geophysical field prediction ${\hat{\mathit{X}}}_{k/k-1}$ is obtained from the propagation equation constructed by the gyroscope-sensed angular rate ${\tilde{\mathit{\omega }}}_{k-1}^b$ at time $k$, and the geophysical field measurement ${\mathit{Z}}_k$ is obtained from the accelerometer-sensed specific force ${\tilde{\mathit{f}}}_k^b$ and the magnetometer-sensed magnetic vector ${\tilde{\mathit{m}}}_k^b$. The measurement noise covariance matrix ${\hat{\mathit{R}}}_k$ is adaptively estimated in a sequential structure according to ${\tilde{\mathit{Z}}}_{k/k-1}$, which is calculated by ${\hat{\mathit{X}}}_{k/k-1}$ and ${\mathit{Z}}_k$. Finally, the geophysical field estimation ${\hat{\mathit{X}}}_k$ is obtained, and the Euler angle can be further calculated based on it. The block diagram of the SA-GKF algorithm is shown in Figure 1. In this section, the block diagram will be explained explicitly.

3.1. System Model

3.1.1. Process Model

The rotation matrix of frame {$b$} at time $k$ relative to frame {$b$} at time $k-1$, i.e., ${\mathit{C}}_{b(k-1)}^{b(k)}$, can be expressed as follows:

$${\mathit{C}}_{b(k-1)}^{b(k)}=\mathit{I}-\left(\mathit{\varphi }\times \right),$$

(6)

where $\mathit{I}$ stands for the three-order identity matrix; $\mathit{\varphi }$ stands for the rotation vector of frame {$b$} at time $k$ relative to frame {$b$} at time $k-1$, which can be expressed by the calibrated gyroscope-sensed angular rate ${\tilde{\mathit{\omega }}}_{k-1}^b={\left[\begin{array}{ccc}{\tilde{\omega }}_{x|k-1}^b& {\tilde{\omega }}_{y|k-1}^b& {\tilde{\omega }}_{z|k-1}^b\end{array}\right]}^T$ and gyroscope noise ${\mathit{w}}_{g|k-1}^b$ in frame {$b$} at time $k-1$:

$$\mathit{\varphi }={\tilde{\mathit{\omega }}}_{k-1}^b\Delta t-{\mathit{w}}_{g|k-1}^b\Delta t,$$

(7)

where $\Delta t$ stands for the sampling interval of gyroscopes.

Hence, the specific force in frame {$b$}’s transformation between time $k$ and time $k-1$ can be expressed using Equations (6) and (7), i.e., [23]

$$\begin{array}{ll}{\mathit{f}}_k^b& ={\mathit{C}}_{b(k-1)}^{b(k)}{\mathit{f}}_{k-1}^b\\ & =\left[I-\left(\mathit{\varphi }\times \right)\right]{\mathit{f}}_{k-1}^b\\ & =\left\{I-\left[\left({\tilde{\mathit{\omega }}}_{k-1}^b\Delta t\right)\times \right]\right\}{\mathit{f}}_{k-1}^b-\left({\mathit{f}}_{k-1}^b\times \right){\mathit{w}}_{g|k-1}^b\Delta t\end{array}.$$

(8)

Similarly, the magnetic vector in frame {$b$}’s transformation between time $k$ and time $k-1$ can be obtained as follows [23]:

$${\mathit{m}}_k^b=\left\{I-\left[\left({\tilde{\mathit{\omega }}}_{k-1}^b\Delta t\right)\times \right]\right\}{\mathit{m}}_{k-1}^b-\left({\mathit{m}}_{k-1}^b\times \right){\mathit{w}}_{g|k-1}^b\Delta t.$$

(9)

In summary, the process model of the SA-GKF algorithm can be abbreviated as follows:

$${\mathit{X}}_k={\mathit{\Phi }}_{k/k-1}{\mathit{X}}_{k-1}+{\mathit{\Gamma }}_{k-1}{\mathit{W}}_{k-1}$$

(10)

where

$$\left\{\begin{array}{l}{\mathit{X}}_k=\left[\begin{array}{c}{\mathit{f}}_k^b\\ {\mathit{m}}_k^b\end{array}\right], {\mathit{X}}_{k-1}=\left[\begin{array}{c}{\mathit{f}}_{k-1}^b\\ {\mathit{m}}_{k-1}^b\end{array}\right], {\mathit{W}}_{k-1}=\left[\begin{array}{c}{\mathit{w}}_{g|k-1}^b\\ {\mathit{w}}_{g|k-1}^b\end{array}\right]\hfill \\ {\mathit{\Phi }}_{k/k-1}=\mathrm{diag}\left(I-\left[\left({\tilde{\mathit{\omega }}}_{k-1}^b\Delta t\right)\times \right], I-\left[\left({\tilde{\mathit{\omega }}}_{k-1}^b\Delta t\right)\times \right]\right)\hfill \\ {\mathit{\Gamma }}_{k-1}=\mathrm{diag}\left(-\left({\mathit{f}}_{k-1}^b\times \right)\Delta t,-\left({\mathit{m}}_{k-1}^b\times \right)\Delta t\right)\hfill \end{array}\right..$$

(11)

In Equation (11), ${\mathit{X}}_k$ and ${\mathit{X}}_{k-1}$ are the state vector at time $k$ and time $k-1$, respectively; ${\mathit{\Phi }}_{k/k-1}$ is the state one-step transition matrix; ${\mathit{\Gamma }}_{k-1}$ is the process noise distribution matrix; and ${\mathit{W}}_{k-1}$ is the process noise vector. The covariance matrix of ${\mathit{W}}_{k-1}$ is defined as ${\mathit{Q}}_{k-1}$, which can be expressed as follows:

$${\mathit{Q}}_{k-1}=\mathrm{diag}\left(\begin{array}{ccc}{\delta }_{g|x}^2& {\delta }_{g|y}^2& {\delta }_{g|z}^2\end{array}\right),$$

(12)

where ${\delta }_{g|x}^2$, ${\delta }_{g|y}^2$, and ${\delta }_{g|z}^2$ are the variances of ${\tilde{\omega }}_x^b$, ${\tilde{\omega }}_y^b$, and ${\tilde{\omega }}_z^b$, which can be calculated by collecting gyroscope measurements for a long time under static conditions.

${\mathit{Q}}_{k-1}$ significantly affects filtering performance [35]. An undersized ${\mathit{Q}}_{k-1}$ makes the filter over-rely on the model and respond slowly to real state changes, whereas an oversized ${\mathit{Q}}_{k-1}$ causes excessive dependence on measurements and degrades estimation smoothness.

3.1.2. Measurement Model

The calibrated accelerometer-sensed specific force and the calibrated magnetometer-sensed magnetic vector are defined as ${\tilde{\mathit{f}}}_k^b={\left[\begin{array}{ccc}{\tilde{f}}_{x|k}^b& {\tilde{f}}_{y|k}^b& {\tilde{f}}_{z|k}^b\end{array}\right]}^T$ and ${\tilde{\mathit{m}}}_k^b={\left[\begin{array}{ccc}{\tilde{m}}_{x|k}^b& {\tilde{m}}_{y|k}^b& {\tilde{m}}_{z|k}^b\end{array}\right]}^T$, which can be expressed by ${\mathit{X}}_k$ as follows [23]:

$$\left[\begin{array}{c}{\tilde{\mathit{f}}}_k^b\\ {\tilde{\mathit{m}}}_k^b\end{array}\right]=\left[\begin{array}{c}{\mathit{f}}_k^b\\ {\mathit{m}}_k^b\end{array}\right]+\left[\begin{array}{c}{\mathit{w}}_{a|k}^b\\ {\mathit{w}}_{m|k}^b\end{array}\right],$$

(13)

where ${\mathit{w}}_{a|k}^b$ is the accelerometer noise and ${\mathit{w}}_{m|k}^b$ is the magnetometer noise.

Therefore, the measurement model of the SA-GKF algorithm can be abbreviated as follows:

$${\mathit{Z}}_k={\mathit{H}}_k{\mathit{X}}_{k-1}+{\mathit{V}}_{k-1},$$

(14)

where

$$\left\{\begin{array}{l}{\mathit{Z}}_k=\left[\begin{array}{c}{\tilde{\mathit{f}}}_k^b\\ {\tilde{\mathit{m}}}_k^b\end{array}\right],{\mathit{X}}_k=\left[\begin{array}{c}{\mathit{f}}_k^b\\ {\mathit{m}}_k^b\end{array}\right]\hfill \\ {\mathit{H}}_k={\mathit{I}}_{6\times 6},{\mathit{V}}_k=\left[\begin{array}{c}{\mathit{w}}_{a|k}^b\\ {\mathit{w}}_{m|k}^b\end{array}\right]\hfill \end{array}\right..$$

(15)

In Equation (15), ${\mathit{Z}}_k$ is the measurement vector at time $k$; ${\mathit{H}}_k$ is the measurement matrix; and ${\mathit{V}}_k$ is the measurement noise vector. The covariance matrix of ${\mathit{V}}_k$ is defined as ${\mathit{R}}_k$, which can be expressed as follows:

$${\mathit{R}}_k=\mathrm{diag}\left(\begin{array}{llllll}{\delta }_{a|x}^2\hfill & {\delta }_{a|y}^2\hfill & {\delta }_{a|z}^2\hfill & {\delta }_{m|x}^2\hfill & {\delta }_{m|y}^2\hfill & {\delta }_{m|z}^2\hfill \end{array}\right),$$

(16)

where ${\delta }_{a|x}^2$, ${\delta }_{a|y}^2$, ${\delta }_{a|z}^2$, ${\delta }_{m|x}^2$, ${\delta }_{m|y}^2$, and ${\delta }_{m|z}^2$ are the variances of ${\tilde{f}}_x^b$, ${\tilde{f}}_y^b$, ${\tilde{f}}_z^b$, ${\tilde{m}}_x^b$, ${\tilde{m}}_y^b$, and ${\tilde{m}}_z^b$, which can be calculated by collecting accelerometer and magnetometer measurements for a long time under static and stable conditions.

3.1.3. System State Space Model

From Equations (10) and (14), we can obtain the system state space model of the SA-GKF algorithm as follows:

$$\left\{\begin{array}{l}{\mathit{X}}_k={\mathit{\Phi }}_{k/k-1}{\mathit{X}}_{k-1}+{\mathit{\Gamma }}_{k-1}{\mathit{W}}_{k-1}\hfill \\ {\mathit{Z}}_k={\mathit{H}}_k{\mathit{X}}_{k-1}+{\mathit{V}}_{k-1}\hfill \end{array}\right..$$

(17)

3.2. SA-GKF Algorithm

3.2.1. Time Update

According to the state estimation ${\mathit{X}}_{k-1}$ at time $k-1$ and the process model in Equation (10), the state at time $k$, i.e., the process of state one-step prediction, can be predicted as follows:

$${\hat{\mathit{X}}}_{k/k-1}={\mathit{\Phi }}_{k/k-1}{\hat{\mathit{X}}}_{k-1},$$

(18)

where ${\mathit{W}}_{k-1}$ is the zero-mean white noise, which contributes nothing to the state prediction.

Then, the state covariance matrix at time $k$ can be predicted by the state estimation covariance matrix at time $k-1$, i.e., the process of the one-step state prediction covariance matrix, which is defined as follows:

$${\mathit{P}}_{k/k-1}={\mathit{\Phi }}_{k/k-1}{\mathit{P}}_{k-1}{\mathit{\Phi }}_{k/k-1}^T+{\mathit{\Gamma }}_{k-1}{\mathit{Q}}_{k-1}{\mathit{\Gamma }}_{k-1}^T.$$

(19)

3.2.2. Adaptive Measurement Update

The external acceleration and magnetic anomalies can significantly affect the estimation accuracy of the inclination and yaw, respectively. Therefore, adaptive algorithms must be used to solve the interference problem. The Sage–Husa adaptive filter algorithm is commonly used in practice, and the specific implementation is as follows [32]:

$$\left\{\begin{array}{l}{\tilde{\mathit{Z}}}_{k/k-1}={\mathit{Z}}_k-{\mathit{H}}_k{\hat{\mathit{X}}}_{k/k-1}\hfill \\ {\beta }_k={\beta }_{k-1}/\left({\beta }_{k-1}+\tau \right)\hfill \\ {\hat{\mathit{R}}}_k=(1-{\beta }_k){\hat{\mathit{R}}}_{k-1}+{\beta }_k({\tilde{\mathit{Z}}}_{k/k-1}{\tilde{\mathit{Z}}}_{k/k-1}^T-{\mathit{H}}_k{\mathit{P}}_{k/k-1}{\mathit{H}}_k^T)\hfill \end{array}\right.,$$

(20)

where ${\tilde{\mathit{Z}}}_{k/k-1}$ is the measurement innovation; $\tau \in \left[0.9, 0.999\right]$ is the fading factor, so that the filter can always maintain the adaptive ability of ${\hat{\mathit{R}}}_k$ [36]. Based on a series of comparative experiments, $\tau$ was set to 0.95.

However, the Sage–Husa adaptive filter is sensitive to the initial value selection. If the actual measurement noise is too low compared to the theoretical modeling value or the initial state noise setting is too high, ${\hat{\mathit{R}}}_k$ may lose its positive definiteness and cause a filter anomaly [32].

In addition, in practical applications, the number of adaptive parameters should be reduced to maintain the reliability of the filter. A simple and effective method is to use a sequential structure to reduce adaptive parameters and impose upper and lower boundary limits on each diagonal component of ${\hat{\mathit{R}}}_k$ to ensure that it remains within a reasonable range.

From Equation (14), the measurement equation at time $k$ can be decomposed into six groups as follows:

$$\left[\begin{array}{l}{\mathit{Z}}_k^{(1)}\hfill \\ {\mathit{Z}}_k^{(2)}\hfill \\ {\mathit{Z}}_k^{(3)}\hfill \\ {\mathit{Z}}_k^{(4)}\hfill \\ {\mathit{Z}}_k^{(5)}\hfill \\ {\mathit{Z}}_k^{(6)}\hfill \end{array}\right]=\left[\begin{array}{l}{\mathit{H}}_k^{(1)}\hfill \\ {\mathit{H}}_k^{(2)}\hfill \\ {\mathit{H}}_k^{(3)}\hfill \\ {\mathit{H}}_k^{(4)}\hfill \\ {\mathit{H}}_k^{(5)}\hfill \\ {\mathit{H}}_k^{(6)}\hfill \end{array}\right]{\mathit{X}}_k+\left[\begin{array}{l}{\mathit{V}}_k^{(1)}\hfill \\ {\mathit{V}}_k^{(2)}\hfill \\ {\mathit{V}}_k^{(3)}\hfill \\ {\mathit{V}}_k^{(4)}\hfill \\ {\mathit{V}}_k^{(5)}\hfill \\ {\mathit{V}}_k^{(6)}\hfill \end{array}\right].$$

(21)

The measurement noise covariance matrix can be expressed in the following component block diagonal matrix form:

$${\mathit{R}}_k=\mathrm{diag}\left({\mathit{R}}_k^{(1)},{\mathit{R}}_k^{(2)},{\mathit{R}}_k^{(3)},{\mathit{R}}_k^{(4)},{\mathit{R}}_k^{(5)},{\mathit{R}}_k^{(6)}\right).$$

(22)

Therefore, when the i-th scalar sequential measurement is updated, the scalar measurement equation is

$${\mathit{Z}}_k^{(i)}={\mathit{H}}_k^{(i)}{\mathit{X}}_k+{\mathit{V}}_k^{(i)}.$$

(23)

Further, the i-th scalar sequential measurement innovation can be expressed as follows:

$${\tilde{\mathit{Z}}}_{k/k-1}^{(i)}={\mathit{Z}}_k^{(i)}-{\mathit{H}}_k^{(i)}{\hat{\mathit{X}}}_k.$$

(24)

The i-th scalar sequential measurement innovation covariance is

$${\mathit{\rho }}_k^{(i)}={({\tilde{\mathit{Z}}}_{k/k-1}^{(i)})}^2-{\mathit{H}}_k^{(i)}{\mathit{P}}_{k/k-1}^{(i)}{({\mathit{H}}_k^{(i)})}^T.$$

(25)

Equation (25) is used for setting upper and lower boundary restrictions with the following conditions on each ${\hat{\mathit{R}}}_k$ diagonal component [32]:

$${\hat{\mathit{R}}}_k^{(i)}=\left\{\begin{array}{ll}(1-{\beta }_k){\hat{\mathit{R}}}_{k-1}^{(i)}\hfill & {\mathit{\rho }}_k^{(i)}<0\hfill \\ (1-{\beta }_k){\hat{\mathit{R}}}_{k-1}^{(i)}+{\beta }_k{\mathit{\rho }}_k^{(i)}\hfill & 0\le {\mathit{\rho }}_k^{(i)}\le {\hat{\mathit{R}}}_{\max }^{(i)}\hfill \\ {\hat{\mathit{R}}}_{\max }^{(i)}\hfill & {\mathit{\rho }}_k^{(i)}>{\hat{\mathit{R}}}_{\max }^{(i)}\hfill \end{array}\right.,$$

(26)

where the lower boundary $0$ is used to ensure the positive quality of ${\hat{\mathit{R}}}_k^{(i)}$, and the upper boundary ${\hat{\mathit{R}}}_{\max }^{(i)}$ is used to quickly reduce the reliability of measurement information ${\mathit{Z}}_k^{(i)}$ and test whether ${\mathit{Z}}_k^{(i)}$ is abnormal. If ${\mathit{\rho }}_k^{(i)}>{\hat{\mathit{R}}}_{\max }^{(i)}$, ${\mathit{Z}}_k^{(i)}$ is regarded as abnormal, and the measurement update is abandoned.

Through the above processing, the system noise parameters can be estimated quickly in real time, and the impact of external acceleration and magnetic anomalies on the inclination and yaw estimation can be suppressed, with good adaptability and reliability.

The i-th sequential filter gain is the weight matrix that balances the contribution of the predicted and measured state values, i.e.,

$${\mathit{K}}_k^{(i)}={\mathit{P}}_{k/k-1}^{(i)}{\left({\mathit{H}}_k^{(i)}\right)}^T{\left[{\mathit{H}}_k^{(i)}{\mathit{P}}_{k/k-1}^{(i)}{\left({\mathit{H}}_k^{(i)}\right)}^T+{\mathit{R}}_k^{(i)}\right]}^{-1},$$

(27)

where ${\mathit{P}}_{k/k-1}^{(i)}$ is the i-th sequential one-step state prediction covariance matrix.

The i-th sequential state at time $k$ can be further modified by ${\mathit{K}}_k^{(i)}$ on the basis of the i-th sequential state prediction ${\hat{\mathit{X}}}_{k/k-1}^{(i)}$, i.e., the process of state estimation, as follows:

$${\hat{\mathit{X}}}_k^{(i)}={\hat{\mathit{X}}}_{k/k-1}^{(i)}+{\mathit{K}}_k^{(i)}\left({\mathit{Z}}_k^{(i)}-{\mathit{H}}_k^{(i)}{\hat{\mathit{X}}}_{k/k-1}^{(i)}\right).$$

(28)

The i-th sequential state estimation covariance matrix can be obtained as follows:

$${\mathit{P}}_k^{(i)}=\left(\mathit{I}-{\mathit{K}}_k^{(i)}{\mathit{H}}_k^{(i)}\right){\mathit{P}}_{k/k-1}^{(i)}.$$

(29)

To sum up, the final filtered value is

$$\left\{\begin{array}{l}{\hat{\mathit{X}}}_k={\hat{\mathit{X}}}_k^{(n)}\hfill \\ {\mathit{P}}_k={\mathit{P}}_k^{(n)}\hfill \end{array}\right.,$$

(30)

where $n$ is the dimension of the measurement vector.

3.3. Euler Angle Estimation

In summary, the Euler angle estimation ${\hat{\mathit{A}}}_k={\left[\begin{array}{ccc}{\widehat{\theta }}_k& {\widehat{\gamma }}_k& {\widehat{\psi }}_k\end{array}\right]}^T$ can be obtained from (3) and (5) and Table 1 and Table 2 by ${\hat{\mathit{X}}}_k$.

The proposed SA-GKF algorithm runs recursively by combining (3), (5), (11)–(12), (15)–(16), and (18)–(30). The implementation pseudocode of the SA-GKF algorithm is shown in Algorithm 1.

Algorithm 1: Sequential adaptive geophysical-field-based Kalman filter

Input ${\mathit{\Phi }}_{k/k-1}=\mathrm{diag}\left(I-\left[\left({\mathit{\omega }}_{k-1}^b\Delta t\right)\times \right], I-\left[\left({\mathit{\omega }}_{k-1}^b\Delta t\right)\times \right]\right)$, ${\mathit{\Gamma }}_{k-1}=\mathrm{diag}\left(\left({\mathit{f}}_{k-1}^b\times \right)\Delta t, \left({\mathit{m}}_{k-1}^b\times \right)\Delta t\right)$, ${\mathit{H}}_k={\mathit{I}}_{6\times 6}$, ${\mathit{X}}_{k-1}=\left[\begin{array}{c}{\mathit{f}}_{k-1}^b\\ {\mathit{m}}_{k-1}^b\end{array}\right]$, ${\mathit{Z}}_k=\left[\begin{array}{c}{\tilde{\mathit{f}}}_k^b\\ {\tilde{\mathit{m}}}_k^b\end{array}\right]$, ${\mathit{Q}}_{k-1}$, ${\mathit{R}}_{k-1}$, ${\beta }_{k-1}$, $\tau$, $n$. 1: ${\hat{\mathit{X}}}_{k/k-1}={\mathit{\Phi }}_{k/k-1}{\hat{\mathit{X}}}_{k-1}$; //State-one-step prediction. 2: ${\mathit{P}}_{k/k-1}={\mathit{\Phi }}_{k/k-1}{\mathit{P}}_{k-1}{\mathit{\Phi }}_{k/k-1}^T+{\mathit{\Gamma }}_{k-1}{\mathit{Q}}_{k-1}{\mathit{\Gamma }}_{k-1}^T$; //State-one-step prediction covariance matrix. 3: ${\beta }_k={\beta }_{k-1}/\left({\beta }_{k-1}+\tau \right)$; For i = 1:n 4: ${\mathit{Z}}_k^{(i)}={\mathit{H}}_k^{(i)}{\mathit{X}}_k+{\mathit{V}}_k^{(i)}$; //the i-th measurement. 5: ${\tilde{\mathit{Z}}}_{k/k-1}^{(i)}={\mathit{Z}}_k^{(i)}-{\mathit{H}}_k^{(i)}{\hat{\mathit{X}}}_k$; //the i-th measurement innovation. 6: ${\mathit{\rho }}_k^{(i)}={({\tilde{\mathit{Z}}}_{k/k-1}^{(i)})}^2-{\mathit{H}}_k^{(i)}{\mathit{P}}_{k/k-1}^{(i)}{({\mathit{H}}_k^{(i)})}^T$; //the i-th measurement innovation covariance. 7: Calculate ${\hat{\mathit{R}}}_k^{(i)}$ from (24); //the i-th measurement covariance. 8: if ${\mathit{\rho }}_k^{(i)}>{\hat{\mathit{R}}}_{\max }^{(i)}$, ${\hat{\mathit{X}}}_k^{(i)}={\hat{\mathit{X}}}_{k/k-1}^{(i)}$; else ${\mathit{K}}_k^{(i)}={\mathit{P}}_{k/k-1}^{(i)}{\left({\mathit{H}}_k^{(i)}\right)}^T{\left[{\mathit{H}}_k^{(i)}{\mathit{P}}_{k/k-1}^{(i)}{\left({\mathit{H}}_k^{(i)}\right)}^T+{\mathit{R}}_k^{(i)}\right]}^{-1}$, $,{\hat{\mathit{X}}}_k^{(i)}={\hat{\mathit{X}}}_{k/k-1}^{(i)}+{\mathit{K}}_k^{(i)}\left({\mathit{Z}}_k^{(i)}-{\mathit{H}}_k^{(i)}{\hat{\mathit{X}}}_{k/k-1}^{(i)}\right)$; //Measurement fault isolation. 9: ${\mathit{P}}_k^{(i)}=\left(\mathit{I}-{\mathit{K}}_k^{(i)}{\mathit{H}}_k^{(i)}\right){\mathit{P}}_{k/k-1}^{(i)}$; //Measurement fault isolation. End 10: ${\hat{\mathit{X}}}_k={\hat{\mathit{X}}}_k^{(n)}$, ${\mathit{P}}_k={\mathit{P}}_k^{(n)}$; //Final filtered value. 11: Calculate ${\hat{\theta }}_k$, ${\hat{\gamma }}_k$, and ${\hat{\psi }}_k$ from (3), (5), Table 1 and Table 2 by ${\hat{\mathit{X}}}_k$; //Inclination and yaw calculation. 12: ${\hat{\mathit{A}}}_k={\left[\begin{array}{ccc}{\widehat{\theta }}_k& {\widehat{\gamma }}_k& {\widehat{\psi }}_k\end{array}\right]}^T$. Output ${\beta }_k$, ${\mathit{R}}_k$, ${\mathit{X}}_k$, ${\mathit{P}}_k$, ${\hat{\mathit{A}}}_k$.

For the sequential Sage–Husa adaptive linear Kalman filter with a state vector of dimension $n$ and measurement vector of dimension $m$, the computational complexity is $O\left(n^3+n{m}^2\right)$, and the memory complexity is $O\left(n^2+m^2+nm\right)$. The proposed SA-GKF algorithm, with $n=m=6$, has been successfully deployed on an embedded system based on an the STM32F411RET6 microcontroller produced by STMicroelectronics in Geneva, Switzerland (100 MHz main frequency, 128 KB SRAM) at an execution frequency of 100 Hz, demonstrating its feasibility and efficiency in resource-constrained systems.

In summary, the SA-GKF algorithm combines geophysical field modeling with the sequential adaptive filter to achieve strong robustness against external acceleration and magnetic anomalies. The effectiveness of this algorithm will be verified by the experiments presented below.

4. Experiments and Discussion

In order to verify the effectiveness of the proposed SA-GKF algorithm, a set of experiments was designed. This section presents the experimental platform, design, process, results, and discussion.

4.1. Experimental Platform

In order to verify the effectiveness of the SA-GKF algorithm, a set of experiments were designed based on our self-developed MARG-AHRS module [37], which integrates with XV7001BB gyroscopes (produced by Epson in Suwa, Japan), LIS3LV02DL accelerometers (produced by STMicroelectronics in Geneva, Switzerland), RM3100 magnetometers (produced by PNI in Windsor, CT, USA), a LM75A thermometer (produced by Texas Instruments in Dallas, TX, USA), a STM32F411RET6 microcontroller, and a high-precision orthogonal housing (see Figure 2a). Gyroscopes, accelerometers, and magnetometers are calibrated by the multi-angle-rate method [38], the six-positions method [39], and the error separation method [40], respectively. After testing, the root mean square error of the calibrated gyroscope measurement, accelerometer measurement, and magnetometer measurement are 0.2°/s, 0.0004 g, and 0.003 h. Under static conditions, the inclination accuracy is 0.25°, and the yaw accuracy is 0.3°. Only calibrated MARG data fusion can ensure the stability of the filter. In the experiment, to ensure the effectiveness of the generated measurement interference, the attitude error caused by external acceleration and magnetic anomalies will be made greater than the module’s attitude accuracy. The sampling rate of the module is 100 Hz, and the main performance parameters of the sensors in the MARG module are shown in

Table 3. Main performance parameters of the MARG module.

Sensor	Range	Sensitivity
XV7001BB	±100°/s	70 LSB/(°/s)
LIS3LV02DL	±2 g	1024 LSB/g
RM3100	±800 μT	38 LSB/μT

. In addition, an experimental platform based on a three-axis non-magnetic turntable and the upper computer was built (see Figure 2b).

4.2. Experimental Design

A 0.02 mm/m air-level was used to strictly level the yaw axis (i.e., Z-axis), pitch axis (i.e., X-axis), and roll axis (i.e., Y-axis) of the three-axis non-magnetic turntable. Then, the MARG-AHRS module was fixed on the turntable plane to execute the experiment.

Two sets of experiments were designed: (a) In the pitch dynamic experiment, the X-axis of the turntable was manually rotated, and the MARG-AHRS module’s outputs of roll and yaw were recorded. (b) In the roll dynamic experiment, the Y-axis of the turntable was manually rotated, and the MARG-AHRS module’s outputs of pitch and yaw were ecorded. In these experiments, external acceleration and magnetic anomalies were realized by rotating the X-/Y-axes of the three-axis non-magnetic turntable and introducing a magnet.

At first, the effectiveness of the SA-GKF algorithm is verified by comparing it with the GKF algorithm (i.e., an SA-GKF algorithm without an adaptive strategy). Secondly, the rationality of choosing an SA strategy is verified by comparing the SA-GKF algorithm with the threshold-based adaptive GKF (TA-GKF) algorithm, Chi-square test adaptive GKF (CA-GKF) algorithm, and Variational Bayesian adaptive GKF (VBA-GKF) algorithm. Finally, the advancement of the SA-GKF algorithm is verified by comparing it with the DOEA and the RESKF algorithm.

The quantitative index of attitude estimation robustness is the maximum error of the Euler angle. The Euler angle errors were obtained from the difference between the reference value and the algorithm estimate. In the experimental results, Reference stands for the Euler angles reference value.

4.3. Anti-Interference Experiments

4.3.1. Pitch Dynamic Experiment

In this experiment, the Y-axis and Z-axis were kept stationary. The reference values of roll are zero, and the reference value of yaw is the scale value of the magnetic north, corresponding to the turntable’s 142.45°.

The magnitudes of the specific force and magnetic vector in the pitch dynamic experiment are shown in Figure 3a,b, respectively.

As shown in Figure 4 and

Table 4. Pitch dynamic experiment results.

Algorithm	Roll Maximum Error	Yaw Maximum Error
SA-GKF (the proposed)	0.3703°	0.4964°
TA-GKF	0.4241°	1.8442°
CA-GKF	0.4920°	0.6979°
VBA-GKF	0.4226°	1.5056°
DOEA	0.3716°	1.4666°
RESKF	0.3879°	0.5443°
GKF	0.4692°	13.1888°

, in the pitch dynamic experiment, compared with the GKF algorithm, the SA-GKF algorithm’s maximum roll error is reduced by 21.08%, and its maximum yaw error is reduced by 96.24%. In addition, compared with the TA-GKF algorithm, the CA-GKF algorithm, the VBA-GKF algorithm, the DOEA, and the RESKF algorithm, the SA-GKF algorithm works better and is more stable.

4.3.2. Roll Dynamic Experiment

In this experiment, the X-axis and Z-axis were kept stationary. The reference values of the pitch are zero, and the reference value of yaw is the scale value of the magnetic north, corresponding to the turntable’s 142.45°.

The magnitudes of the specific force and magnetic vector in the pitch dynamic experiment are shown in Figure 5a,b, respectively.

As shown in Figure 6 and

Table 5. Roll dynamic experiment results.

Algorithm	Pitch Maximum Error	Yaw Maximum Error
SA-GKF (the proposed model)	0.2028°	0.3918°
TA-GKF	0.2492°	2.0767°
CA-GKF	0.2561°	0.8372°
VBA-GKF	0.2430°	0.7669°
DOEA	0.2500°	0.4468°
RESKF	0.2164°	0.4047°
GKF	0.2939°	6.8134°

, in the roll dynamic experiment, compared with the GKF algorithm, the SA-GKF algorithm’s maximum pitch error is reduced by 31.00%, and its maximum yaw error is reduced by 94.26%. In addition, compared with the TA-GKF algorithm, the CA-GKF algorithm, the VBA-GKF algorithm, the DOEA, and the RESKF algorithm, the SA-GKF algorithm works better and is more stable.

4.4. Experimental Discussions

From the experimental results in Section 4.3, we can see that the proposed SA-GKF algorithm can effectively suppress the influence of external acceleration and soft/hard magnetic anomalies on the estimation inclination and yaw estimation, respectively. When interference occurs, the SAGKF algorithm constructs the measurement innovation in real time by calculating the difference between the prediction and measurement of the geophysical fields. Then, the SAGKF algorithm estimates the measurement noise covariance matrix and combines the fault isolation to shield abnormal measurements, rapidly reducing the credibility of abnormal measurements and significantly enhancing the robustness of MARG attitude estimation.

Furthermore, compared with the TA, CA, and VBA strategies, the SA strategy exhibits stronger adaptability and is more suitable for combination with the GKF algorithm. The TA and CA strategies suffer from the binarity problem, making it difficult to determine the appropriate threshold. Therefore, both are prone to false alarms or missed detections, which can result in filter anomaly or even divergence. The VBA strategy relies on the factorization assumption and has no boundary constraints, resulting in reduced stability when the noise characteristics vary rapidly.

Finally, the experimental results show that the SA-GKF algorithm strategy has a higher attitude estimation accuracy than the DOEA and the RESKF algorithm, showing that the SA-GKF algorithm has certain advantages. The adaptive strategy of the DOEA is consistent with the TA strategy, and therefore DOEA has the same binarity problem. In the RESKF algorithm, both the state and measurement vectors are represented in the Euler angle, making it difficult to accurately model the noise statistics, which further limits its estimation precision.

The above analysis confirms that the proposed SA-GKF algorithm achieves superior performance and robustness.

5. Conclusions

Based on the theoretical analysis and experimental validation presented in the previous sections, the main contributions and future perspectives are summarized as follows:

In this work, a sequential adaptive linear Kalman filter algorithm based on the geophysical field for MARG attitude estimation was proposed, which suppresses the maximum error of yaw estimation by more than 94% and that of inclination estimation by more than 21%. Compared with the existing three adaptive strategies and two algorithms, the SA-GKF represents certain advancements. The SA-GKF algorithm can provide a strongly robust MARG attitude estimation solution for resource exploration, navigation and positioning, motion control, and stone and earthen materials testing, promoting the development of related industries.

However, the SA-GKF algorithm only performs online estimation of the measurement noise covariance matrix, without considering the effect of inaccuracies in the process noise covariance matrix. Moreover, when long-term disturbances occur, the filter relies solely on the time update process. After the disturbance ends, the filter may have already diverged, causing normal measurements to be misidentified as abnormal ones, thereby preventing the system from returning to stable filtering. In future work, the adaptive MARG attitude estimation algorithm will be studied further under conditions where both measurement noise and process noise are uncertain. In addition, long-term stable magnetic interference can be suppressed by combining the algorithm with the updated magnetic declination. Both are conducive to preventing filter divergence and improving the robustness of MARG attitude estimation.