The Novel Gravity-Matching Algorithm Based on Modified Adaptive Transformed Cubature Quaternion Estimation for Underwater Navigation

Abstract

Gravity matching is a key technology in gravity-aided inertial navigation. The traditional Sandia inertial matching algorithm introduces linearization errors using the linear error model, which can diminish navigation accuracy. To address this issue, we propose a novel gravity-matching algorithm based on modified adaptive transformed cubature quaternion estimation (MA-TCQUE), designed for a nonlinear error model to enhance accuracy in gravity-aided navigation. Additionally, the proposed algorithm can estimate the measurement noise matrix demonstrating improved filtering stability in complex and dynamic environments. Finally, simulation and experimental results validate the advantages of the proposed matching algorithm compared to existing state-of-the-art methods.

1. Introduction

The inertial navigation system (INS) is an autonomous navigation solution known for its high accuracy over short durations, and it has been widely applied in underwater navigation. However, a significant drawback of INS is its tendency for error accumulation over time. To mitigate this issue, it is essential to reduce the error divergence of INS by integrating it with auxiliary navigation systems such as Doppler Velocity Log (DVL), acoustic positioning, and geophysical field positioning [1,2,3]. Gravity-matching navigation, which leverages variations in the Earth’s gravitational field, has become a research focus due to its strong concealment capabilities [4,5,6].

The gravity-aided navigation system consists of an inertial navigation system (INS), a gravimeter, a gravity field background map, and a matching algorithm [7,8]. The matching algorithm is a key technology within this system and is divided into two categories: sequence matching algorithms and single-point matching algorithms [9,10].

The sequence matching algorithm is divided into two main types: the related extremal algorithm and the iterative closest contour point (ICCP) algorithm [11,12,13]. Evolving from the TERCOM algorithm, the related extremal algorithm determines the search range within the marine gravity background field based on position information and error characteristics provided by the INS. It performs a correlation analysis between the gravity anomaly measurement sequence and gravity anomaly data extracted from the gravity background map [14,15]. Subsequently, it identifies the set of points with extrema as estimates of the vehicle’s actual trajectory. However, as a type of a posteriori estimate, this algorithm requires the prior collection of measurement sequences, making it challenging to meet the real-time requirements of navigation systems, and its results are significantly affected by heading errors. In contrast, the ICCP algorithm aligns and matches the measurement sequence with the gravity map by iterating through the nearest points, yielding the nearest point between the contour line and the measurement point [16,17,18]. Generally, the ICCP algorithm offers higher accuracy than the related extremal algorithm, but it is more sensitive to initial INS errors. Both algorithms require a certain number of sampling points before matching, which negatively impacts real-time performance and introduces a delay in the final matching position.

In comparison, single-point matching algorithms offer better real-time performance. The Sandia Inertial Terrain Aided Navigation (SITAN) algorithm is a prominent example of a single-point matching algorithm [19,20]. This algorithm employs the extended Kalman filter (EKF) to adjust position, velocity, and even attitude errors, achieving higher matching precision while maintaining a certain level of real-time performance. However, the linearization of both the state and measurement equations can introduce significant truncation errors, particularly in areas with volatile gravity anomalies. Additionally, the SITAN algorithm has limited anti-interference capabilities. The EKF requires the computation of a Jacobian matrix, which can be complex and cumbersome, thus restricting the algorithm’s applicability. In contrast, the unscented Kalman filter (UKF) approximates the probability density distribution of nonlinear functions, thereby avoiding the introduction of linearization errors. Furthermore, the computational complexity of UKF is similar to that of the EKF, but the UKF typically offers higher accuracy and is better suited for nonlinear problems. As a result, the UKF is integrated into the SITAN algorithm for improved gravity-aided matching performance.

Based on the preceding analysis, the state equation of the SITAN algorithm is derived using a linear error model based on Euler angles, which introduces linearization errors. Consequently, the existing error model for gravity-aided navigation is not suitable, especially when the initial error is large. To address this issue, we propose a quaternion-based nonlinear error model for gravity-aided navigation to enhance accuracy. This proposed model is derived without any approximations, ensuring its performance remains robust, even with significant initial errors. Additionally, quaternions offer advantages in computational efficiency and the avoidance of singularities compared to Euler angles.

In the complex and dynamic underwater environment, gravity sensors often experience various types of interference, leading to a degradation in accuracy. Numerous studies have been conducted to address this challenge. For instance, in [21], an adaptive robust unscented Kalman filter-based matching algorithm is proposed to enhance robustness by incorporating an adaptive factor and a robust function. In [22], a robust two-stage filtering approach for INS/gravity-matching integrated navigation is introduced to improve performance in complex environments. Mao’s research presents an adaptive matching navigation algorithm that utilizes the multi-attribute spatial filtering (MASP) technique to dynamically adjust the filter structure in real time [23,24]. Additionally, Wang proposes a novel, computationally efficient outlier-robust cubature Kalman filter (CKF)-based matching algorithm for underwater gravity-aided navigation systems dealing with outlier-contaminated measurements [25,26,27]. While these methods primarily focus on enhancing robustness in the presence of outliers, achieving high-accuracy positioning is also crucial for gravity-aided navigation and requires further investigation. Although UKF/CKF mitigate linearization errors, their non-local sampling characteristics and fixed noise assumptions remain unstable in dynamic underwater environments. This motivates our integration of adaptive mechanisms and transformed sampling strategies.

Motivated by the aforementioned challenges, this paper proposes a novel gravity-matching algorithm based on modified adaptive transformed cubature quaternion estimation for underwater navigation. First, we derive a quaternion-based nonlinear error model for gravity-aided navigation, which demonstrates superiority over previous linear error models. To implement this nonlinear error model, we utilize modified adaptive transformed cubature quaternion estimation to enhance filtering accuracy and stability. Additionally, the proposed algorithm exhibits improved filtering stability in complex and dynamic environments. Finally, simulation and experimental results highlight the advantages of our method compared to existing algorithms

The remainder of this article is structured as follows: Section 2 presents the problem statement for the gravity-aided navigation system and introduces the quaternion-based nonlinear error model. In Section 3, we propose a modified adaptive transformed cubature quaternion estimation method for gravity-aided navigation. Section 4 verifies the proposed algorithm through simulations and experiments, comparing its performance with existing algorithms. Finally, Section 5 summarizes the paper and presents the conclusions.

2. Problem Statement of Gravity-Matching Navigation System

2.1. Gravity-Aided Navigation System

Gravity-aided INS is an autonomous underwater navigation system that corrects navigation errors from the inertial navigation System (INS). As the underwater vehicle operates, the onboard gravimeter measures gravity anomaly information. Simultaneously, the system retrieves gravity anomaly data from a gravity database based on location information provided by the INS. These two types of information, along with data from the INS, are fused using a matching algorithm to estimate the vehicle’s position, which is then fed back to the INS. Figure 1 illustrates the gravity-matching navigation system.

The above framework demands an accurate state model, which we derive next using quaternion nonlinear equations.

2.2. System Model for Gravity Aided Navigation System

The gravity-aided matching algorithm is implemented based on the INS model, which primarily includes the state–space equation and the measurement equation. We denote the inertial frame as the i frame, the Earth-Centered Earth-Fixed (ECEF) frame as the e frame, the body frame as the b frame, and the navigation frame as the n frame. The error equations for the INS are formulated as follows:

$$\left\{\begin{array}{c}{\dot{\mathbf{\phi }}}^n=-\left({\mathbf{\omega }}_{in}^n\times \right){\mathbf{\phi }}^n+\mathbf{\delta }{\mathbf{\omega }}_{in}^n-{\mathbf{C}}_b^n\mathbf{\delta }{\mathbf{\omega }}_{ib}^b\\ \mathbf{\delta }{\dot{\mathbf{v}}}^n=\left({\mathbf{C}}_b^n{\mathbf{f}}^b\times \right){\mathbf{\phi }}^n-\left[\left(2{\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n\right)\times \right]\mathbf{\delta }{\mathbf{v}}^n-\left[\left(2\mathbf{\delta }{\mathbf{\omega }}_{ie}^n+\mathbf{\delta }{\mathbf{\omega }}_{en}^n\right)\times \right]{\mathbf{v}}^n+{\mathbf{C}}_b^n\mathbf{\delta }{\mathbf{f}}^b\\ \mathbf{\delta }{\dot{\mathbf{p}}}^n={\mathit{M}}_{p\mathrm{v}}\mathbf{\delta }{\mathbf{v}}^n+{\mathit{M}}_{\mathrm{p}\mathrm{p}}\mathbf{\delta }{\mathbf{p}}^n\end{array}\right.$$

(1)

$$\begin{gathered} {\mathit{M}}_{\mathrm{p}\mathrm{v}}=\left[\begin{array}{ccc}0& {\displaystyle \frac{1}{R_M+\mathrm{h}}}& 0\\ {\displaystyle \frac{1}{\left({\mathrm{R}}_N+h\right)\cos L}}& 0& 0\\ 0& 0& 1\end{array}\right] \\ {\mathit{M}}_{\mathrm{p}\mathrm{p}}=\left[\begin{array}{ccc}0& 0& -{\displaystyle \frac{{\mathbf{v}}_N}{{\left({\mathrm{R}}_M+\mathrm{h}\right)}^2}}\\ {\displaystyle \frac{{\mathbf{v}}_E\tan \mathrm{L}}{\left({\mathrm{R}}_N+\mathrm{h}\right)\cos \mathrm{L}}}& 0& -{\displaystyle \frac{{\mathbf{v}}_E}{{\left({\mathrm{R}}_N+\mathrm{h}\right)}^2\cos \mathrm{L}}}\\ 0& 0& 0\end{array}\right] \end{gathered}$$

(2)

where ${\mathbf{C}}_b^n$ denotes the attitude matrix from b frame to n frame, ${\mathbf{v}}^n={\left[{\mathbf{v}}_E^n{\mathbf{v}}_N^n{\mathbf{v}}_U^n\right]}^T$ denotes the velocity in the n frame, and ${\mathbf{p}}^n={\left[L \lambda h\right]}^T$ denotes the position. ${\mathbf{\phi }}^n={\left[{\mathbf{\phi }}_E{\mathbf{\phi }}_N{\mathbf{\phi }}_U\right]}^T$ denotes the misalignment angle, $\mathbf{\delta }{\mathbf{v}}^n={\left[\mathbf{\delta }{\mathbf{v}}_E^n\mathbf{\delta }{\mathbf{v}}_N^n\mathbf{\delta }{\mathbf{v}}_U^n\right]}^T$ denotes the velocity error, and $\mathbf{\delta }{\mathbf{p}}^n={\left[\mathbf{\delta }L \mathbf{\delta }\lambda \mathbf{\delta }h\right]}^T$ denotes the position error. ${\mathbf{\omega }}_{ie}^n$ is the earth rotation rate in the n frame, and ${\mathbf{\omega }}_{en}^n$ denotes the angular rate of the navigation frame with respect to the earth frame in the n frame with ${\mathbf{\omega }}_{in}^n={\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n$. $\mathbf{\delta }{\mathbf{\omega }}_{ie}^n$, $\mathbf{\delta }{\mathbf{\omega }}_{en}^n$, and $\mathbf{\delta }{\mathbf{\omega }}_{in}^n$ denote the calculated errors of ${\mathbf{\omega }}_{ie}^n$, ${\mathbf{\omega }}_{en}^n$, and ${\mathbf{\omega }}_{in}^n$, respectively. ${\mathbf{\omega }}_{ib}^b$ and ${\mathbf{f}}^b$ denote the output of gyro and accelerometer in the b frame, respectively. $\mathbf{\delta }{\mathbf{\omega }}_{ib}^b$ and $\mathbf{\delta }{\mathbf{f}}^b$ denote the constant drift of gyro and accelerometer. $\left(\cdot \times \right)$ denotes the skew-symmetric, where $\mathbf{V}={\left[{\mathbf{V}}_x {\mathbf{V}}_y {\mathbf{V}}_z\right]}^T$ is the vector with the algebras as

$$\left(\mathbf{V}\times \right)=\left[\begin{array}{ccc}0& -{\mathrm{V}}_z& {\mathrm{V}}_y\\ {\mathrm{V}}_z& 0& -{\mathrm{V}}_x\\ -{\mathrm{V}}_y& {\mathrm{V}}_x& 0\end{array}\right]$$

(3)

Define the state vector and state noise vector as follows:

$${\mathbf{X}}_1=\left[{\mathbf{\phi }}^T \mathbf{\delta }{\mathbf{v}}^{nT} \mathbf{\delta }{\mathbf{p}}^T {\mathbf{\epsilon }}^b {\nabla }^b \right]$$

(4)

$${\mathbf{\eta }}_1=\left[{\mathbf{\eta }}_g^b {\mathbf{\eta }}_a^b\right]$$

(5)

According to (1)–(5), the state–space model can be formulated as

$${\dot{\mathbf{X}}}_1={\mathit{F}}_1{\mathbf{X}}_1+{\mathit{G}}_1{\mathbf{\eta }}_1$$

(6)

where ${\mathbf{F}}_1$ is the system dynamic matrix obtained from (1), and ${\mathit{G}}_1$ is the noise matrix.

The gravity anomaly measurement must account for both map interpolation errors and sensor noise, leading to the unified nonlinear model. The difference between the gravity anomaly extracted from the map and the gravimeter measurement is chosen as the measurement, which is expressed as follows

$${\mathbf{Z}}_k=\Delta {\mathrm{g}}_m-\Delta {\mathrm{g}}_t=\Delta {\mathrm{g}}_r\left({\mathbf{p}}_r\right)-\Delta {\mathrm{g}}_t\left({\mathbf{p}}_{INS}\right)+{\mathbf{\eta }}_m-{\mathbf{\eta }}_t$$

(7)

where $\Delta {\mathrm{g}}_r\left({\mathbf{p}}_r\right)$ is the gravimeter measurement and ${\mathbf{p}}_r$ is the true position. $\Delta {\mathrm{g}}_t\left({\mathbf{p}}_{INS}\right)$ is the gravity anomaly obtained from the background map, and ${\mathbf{p}}_{INS}$ is calculated by INS with ${\mathbf{p}}_r={\mathbf{p}}_{INS}-\mathbf{\delta }\mathbf{p}$. ${\mathbf{\eta }}_m$ is the gravimeter measurement noise, and ${\mathbf{\eta }}_t$ is the searching gravity anomaly deviation. For a better filter design, the model in (7) can be rewritten as follows:

$${\mathbf{Z}}_k=\mathrm{h}\left({\mathbf{x}}_k\right)+{\mathbf{\eta }}_k$$

(8)

where $\mathrm{h}\left({\mathbf{x}}_k\right)=\Delta {\mathrm{g}}_t\left({\mathbf{p}}_{INS}-\delta \mathbf{p}\right)-\Delta {\mathrm{g}}_t\left({\mathbf{p}}_{INS}\right)$ and ${\mathbf{\eta }}_k={\mathbf{\eta }}_m+{\mathbf{\eta }}_t$.

3. Modified Adaptive Gravity-Matching Algorithms for Underwater Navigation

3.1. The Quaternion Based Error Model for Gravity-Aided Navigation

As described in Section 2.1, the existing gravity-aided algorithm is derived according to linear error model based on Euler angles. However, this linearized error model is unreliable because the precision loss of linearization exists. As a result, it is challenging to achieve the high accuracy required for a gravity-aided matching algorithm, particularly with large initial errors. In order to solve this issue, we propose a quaternion-based error model for gravity-aided navigation that eliminates the need for approximation, demonstrating superiority over traditional linear error models.

Then the attitude error differential equation of the quaternion-based nonlinear error model is expressed as follows [8]

$${\dot{\mathbf{q}}}_{n^{\prime }}^n={\displaystyle \frac{1}{2}}{\mathbf{q}}_{n^{\prime }}^n\otimes {\mathbf{\omega }}_{n{n}^{\prime }}^{n^{\prime }}={\displaystyle \frac{1}{2}}\left[\begin{array}{cc}0& -{\left({\mathbf{\omega }}_{n{n}^{\prime }}^{n^{\prime }}\right)}^T\\ {\mathbf{\omega }}_{n{n}^{\prime }}^{n^{\prime }}& -\left({\mathbf{\omega }}_{n{n}^{\prime }}^{n^{\prime }}\times \right)\end{array}\right]{\mathbf{q}}_{n^{\prime }}^n$$

(9a)

$${\mathbf{\omega }}_{n{n}^{\prime }}^{n^{\prime }}=\left({\mathbf{I}}_3-{\left({\mathbf{C}}_{n^{\prime }}^n\right)}^T\right){\tilde{\mathbf{\omega }}}_{in}^n+{\left({\mathbf{C}}_{n^{\prime }}^n\right)}^T\mathbf{\delta }{\mathbf{\omega }}_{in}^n-{\mathbf{C}}_b^n\mathbf{\delta }{\mathbf{\omega }}_{ib}^b$$

(9b)

where ${\mathbf{C}}_{n^{\prime }}^n$ denotes the DCM in the form of Euler angles and ${\mathbf{q}}_{n^{\prime }}^n$ denotes the attitude error in quaternion form expressed as

$${\mathbf{C}}_{n^{\prime }}^n={\mathbf{q}}_{n^{\prime },v}^n{\left({\mathbf{q}}_{n^{\prime },v}^n\right)}^T+{\left({\mathbf{q}}_{n^{\prime },0}^n\right)}^2{\mathbf{I}}_3+2\left({\mathbf{q}}_{n^{\prime },0}^n\right)\left({\mathbf{q}}_{n^{\prime },v}^n\times \right)+{\left({\mathbf{q}}_{n^{\prime },v}^n\times \right)}^2$$

(10)

where ${\mathbf{q}}_{n^{\prime }}^n=\left[{\mathbf{q}}_{n^{\prime },0}^n;{\mathbf{q}}_{n^{\prime },v}^n\right]$ with scalar ${\mathbf{q}}_{n^{\prime },0}^n$ and vector ${\mathbf{q}}_{n^{\prime },v}^n$. Then the velocity error differential equation expressed as follows

$$\begin{array}{ll}\mathbf{\delta }{\dot{\mathbf{v}}}^n=& \left({\mathbf{I}}_3-{\mathbf{C}}_{n^{\prime }}^n\right){\mathbf{C}}_b^{n^{\prime }}{\mathbf{f}}^b+{\mathbf{C}}_{n^{\prime }}^n{\mathbf{C}}_b^{n^{\prime }}\mathbf{\delta }{\mathbf{f}}^b\\ & -\left(2{\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n\right)\times \mathbf{\delta }{\mathbf{v}}^n-\left(2\mathbf{\delta }{\mathbf{\omega }}_{ie}^n+\mathbf{\delta }{\mathbf{\omega }}_{en}^n\right)\times {\mathbf{v}}^n\end{array}$$

(11)

The position error differential is the same as (1). And the state vector and system noise vector are defined as follows:

$${\mathbf{X}}_2=\left[{\mathbf{q}}_{n^{\prime }}^{nT} \mathbf{\delta }{\mathbf{v}}^{nT} \mathbf{\delta }{\mathbf{p}}^T {\mathbf{\epsilon }}^b {\nabla }^b \right]$$

(12)

$${\mathbf{\eta }}_2=\left[{\mathbf{\eta }}_g^b {\mathbf{\eta }}_a^b\right]$$

(13)

The integrated navigation filter employs a quaternion-based nonlinear error model in USQUE [8]. However, USQUE is derived using UT transformation, which introduces the non-local sampling issues inherent in the UKF and CKF. Additionally, the covariance matrix can become non-positive, negatively affecting performance. To address these challenges, we propose the Transformed Cubature Quaternion Estimation (TCQUE), which is designed to resolve the non-local sampling problem and enhance filtering accuracy and stability. The steps of TCQUE are as follows:

• Initialization:

Define the global state vector as $\mathbf{X}={\left[{\mathbf{q}}_{n^{\prime }}^{n\mathrm{T}},{\mathbf{x}}^{e\mathrm{T}}\right]}^{\mathrm{T}}$ and local state vector as $\mathbf{x}={\left[\delta {\mathbf{\sigma }}^{\mathrm{T}},{\mathbf{x}}^{e\mathrm{T}}\right]}^{\mathrm{T}}$, where $\delta \mathbf{q}$ is the attitude error in the form of quaternion corresponding as $\delta \mathbf{\sigma }$ in MRP form and ${\mathbf{X}}^e$ is the other part of error.

• Time Propagation:

Evaluate the CKF cubature point and weight

$$\left\{\begin{array}{l}{\mathbf{\xi }}_i=\sqrt{n}{\left[1\right]}_{2n}\\ {\mathbf{\omega }}_i=1/2n\end{array}\right.$$

(14)

where $n$ is state vector dimension with $i=1,2,\dots ,2n$ and $\mathbf{\xi }$ can be expressed as

$$\mathbf{\xi }=\left[\sqrt{n}{\left[1\right]}_n-\sqrt{n}{\left[1\right]}_n\right]$$

(15)

According to transform cubature theorem, the CKF sampling point is orthogonal to the sampling point of TCKF as:

$${\mathbf{\lambda }}_i={\left[{\Lambda }_{1,i},{\Lambda }_{2,i},\dots ,{\Lambda }_{n,i}\right]}^T$$

(16)

$${\Lambda }_{2r-1,i}=\sqrt{2}\cos \left(\left(2r-1\right)i\pi /n\right)$$

(17)

$${\Lambda }_{2r,i}=\sqrt{2}\sin \left(\left(2r-1\right)i\pi /n\right)$$

(18)

where $r=1,2,\dots ,n/2$, if $n$ is an odd number, ${\Lambda }_{n,i}={\left(-1\right)}^i$.

Then the transform cubature points are evaluated as

$${\mathbf{P}}_{k-1}={\mathbf{S}}_{k-1}{\left({\mathbf{S}}_{k-1}\right)}^T$$

(19)

$${\mathbf{\chi }}_{k-1}^{\left(i\right)}={\mathbf{S}}_{k-1}{\mathbf{\lambda }}_i+{\widehat{\mathbf{x}}}_{k-1}$$

(20)

The sampling point can be expressed as

$${\mathbf{\chi }}_{k-1}^{\left(i\right)}={\left[{\mathbf{\chi }}_{k-1}^{\delta \sigma \left(i\right)\mathrm{T}},{\mathbf{\chi }}_{k-1}^{e\left(i\right)\mathrm{T}}\right]}^{\mathrm{T}}$$

(21)

Compute the quaternion sigma points

$${\mathbf{\chi }}_{k-1}^{\delta \mathrm{q}\left(i\right)}={\left[\delta {\mathrm{q}}_{k-1,0}\left(i\right),\delta {\mathbf{q}}_{k-1,1:3}\left(i\right)\right]}^{\mathrm{T}}$$

(22)

$$\mathbf{\delta }{\mathrm{q}}_{k-1,0}\left(i\right)={\displaystyle \frac{-a{‖{\mathbf{\chi }}_{k-1}^{\mathbf{\delta }\sigma \left(i\right)}‖}^2+l\sqrt{l^2+{\left(1-a\right)}^2{‖{\mathbf{\chi }}_{k-1}^{\mathbf{\delta }\sigma \left(i\right)}‖}^2}}{l^2+{‖{\mathbf{\chi }}_{k-1}^{\mathbf{\delta }\sigma \left(i\right)}‖}^2}}$$

(23)

$${\mathbf{\chi }}_{k-1}^{\mathrm{q}\left(i\right)}={\mathbf{\chi }}_{k-1}^{\mathbf{\delta }\mathrm{q}\left(i\right)}\otimes {\mathbf{q}}_{n^{\prime },k-1}^n$$

(24)

$${\mathbf{X}}_{k-1}^{\left(i\right)}={\left[{\mathbf{\chi }}_{k-1}^{\mathrm{q}\left(i\right)\mathrm{T}},{\mathbf{\chi }}_{k-1}^{e\left(i\right)\mathrm{T}}\right]}^{\mathrm{T}}$$

(25)

$${\mathbf{X}}_{k/k-1}^{\left(i\right)}=\mathit{f}\left({\mathbf{X}}_{k-1}^{\left(i\right)}\right)={\left[{{\mathbf{\chi }}_{k/k-1}^{q\left(i\right)}}^{\mathrm{T}},{{\mathbf{\chi }}_{k/k-1}^{e\left(i\right)}}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(26)

Compute error quaternion sigma points

$${\mathbf{\chi }}_{k/k-1}^{\delta \mathrm{q}\left(i\right)}={\mathbf{\chi }}_{k/k-1}^{\mathrm{q}\left(i\right)}\otimes {\left({\mathbf{\chi }}_{k/k-1}^{\mathrm{q}\left(0\right)}\right)}^{-1}$$

(27)

GRP sigma points can be computed

$${\mathbf{\chi }}_{k/k-1}^{\delta \sigma \left(i\right)}=l{\displaystyle \frac{{\mathbf{\chi }}_{k/k-1,1:3}^{\delta \mathrm{q}\left(i\right)}}{a+{\mathbf{\chi }}_{k/k-1,0}^{\delta \mathrm{q}\left(i\right)}}}$$

(28)

$${\mathbf{\chi }}_{k/k-1}^{\left(i\right)}={\left[{{\mathbf{\chi }}_{k/k-1}^{\delta \sigma \left(i\right)}}^{\mathrm{T}},{\mathbf{\chi }}_{k/k-1}^{e\left(i\right) T}\right]}^{\mathrm{T}}$$

(29)

The predicted mean and covariance can be computed

$${\widehat{\mathbf{x}}}_{k/k-1}={\displaystyle \sum _{i=1}^{2n}{\mathbf{\omega }}_i{\mathbf{\chi }}_{k/k-1}^{\left(i\right)}}$$

(30)

$${\mathbf{P}}_{xx,k/k-1}={\displaystyle \sum _{i=1}^{2n}{\mathbf{\omega }}_i\left({\mathbf{\chi }}_{k/k-1}^{\left(i\right)}-{\widehat{\mathbf{x}}}_{k/k-1}\right){\left({\mathbf{\chi }}_{k/k-1}^{\left(i\right)}-{\widehat{\mathbf{x}}}_{k/k-1}\right)}^{\mathrm{T}}}+{\mathbf{Q}}_{k-1}$$

(31)

• Measurement Update:

The sampling point of TCKF is evaluated with [14,15,16,17] as

$${\mathbf{\chi }}_{k/k-1}^{\left(i\right)\ast }={\left[{{\mathbf{\chi }}_{k/k-1}^{\delta \sigma \left(i\right)\ast }}^{\mathrm{T}},{{\mathbf{\chi }}_{k/k-1}^{e\left(i\right)\ast }}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(32)

Then compute the quaternion sigma points with (22)–(24), which is expressed as

$${\mathbf{\gamma }}_{k/k-1}^{\left(i\right)\ast }={\left[{{\mathbf{\chi }}_{k/k-1}^{\delta q\left(i\right)\ast }}^{\mathrm{T}},{{\mathbf{\chi }}_{k/k-1}^{e\left(i\right)\ast }}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(33)

Evaluate the propagated transform cubature points

$${\mathbf{\eta }}_{k/k-1}^{\left(i\right)}=\mathit{h}\left({\mathbf{\gamma }}_{k/k-1}^{\left(i\right)\ast }\right)$$

(34)

The predicted measurement can be estimated

$${\widehat{\mathbf{y}}}_{k/k-1}={\displaystyle \frac{1}{2n}}{\displaystyle \sum _{i=1}^{2n}{\mathbf{\omega }}_i{\mathbf{\eta }}_{k/k-1}^{\left(i\right)}}$$

(35)

The innovation and cross-covariance matrices can be estimated

$${\mathbf{P}}_{xy,k/k-1}={\displaystyle \sum _{i=0}^{2n}{\mathbf{\omega }}_i\left({\mathbf{\chi }}_{k/k-1}^{\left(i\right)\ast }-{\widehat{\mathbf{x}}}_{k/k-1}\right){\left({\mathbf{\eta }}_{k/k-1}^{\left(i\right)}-{\widehat{\mathbf{x}}}_{k/k-1}\right)}^{\mathrm{T}}}$$

(36)

$${\mathbf{P}}_{yy,k/k-1}={\displaystyle \sum _{i=1}^{2n}{\mathbf{\omega }}_i\left({\mathbf{\eta }}_{k/k-1}^{\left(i\right)}-{\widehat{\mathbf{y}}}_{k/k-1}\right){\left({\mathbf{\eta }}_{k/k-1}^{\left(i\right)}-{\widehat{\mathbf{y}}}_{k/k-1}\right)}^{\mathrm{T}}}+{\mathbf{R}}_k$$

(37)

The Kalman gain can be estimated

$${\mathbf{K}}_k={\mathbf{P}}_{\mathbf{x}y,k/k-1}{\mathbf{P}}_{yy,k/k-1}^{-1}$$

(38)

The update state and corresponding error covariance can be obtained

$${\widehat{\mathbf{x}}}_k={\widehat{\mathbf{x}}}_{k/k-1}+{\mathbf{K}}_k\left({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k/k-1}\right)$$

(39)

$${\mathbf{P}}_k={\mathbf{P}}_{xx,k/k-1}-{\mathbf{K}}_k{\mathbf{P}}_{yy,k/k-1}{\mathbf{K}}_k^{\mathrm{T}}$$

(40)

While TCQUE resolves non-local sampling, its fixed noise matrix remains insufficient in dynamic environments, necessitating our adaptive enhancement.

3.2. The Modified Adaptive Transform Cubature Quaternion Estimation for Gravity-Aided Navigation

The MA-TCQUE algorithm executes five core steps (visualized in Figure 2). Unknown noise parameters can degrade accuracy in gravity-aided navigation systems in time-varying environments. To address this, we propose a modified adaptive filter based on the transformed cubature quaternion estimation for underwater gravity matching. The adaptive strategy steps are as follow:

Define the innovation as:

$${\mathbf{\epsilon }}_k={\mathbf{y}}_k-{\widehat{\mathbf{y}}}_{k/k-1}$$

(41)

Define the residual as:

$${\mathbf{\eta }}_k={\mathbf{y}}_k-{\widehat{\mathbf{y}}}_k$$

(42)

The adaptive filter based on innovation (IAE) is given by

$${\widehat{\mathbf{R}}}_k=\left(1-{\mathrm{d}}_k\right){\widehat{\mathbf{R}}}_{k-1}+{\mathrm{d}}_k({\mathbf{\epsilon }}_k{\mathbf{\epsilon }}_k^T-{\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}})$$

(43)

And the adaptive filter based on residual (RAE) is given by

$${\widehat{\mathbf{R}}}_k=\left(1-{\mathrm{d}}_k\right){\widehat{\mathbf{R}}}_{k-1}+{\mathrm{d}}_k({\mathbf{\eta }}_k{\mathbf{\eta }}_k^T+{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^{\mathrm{T}})$$

(44)

IAE directly estimates noise covariance from innovations ${\mathbf{\epsilon }}_k$ but risks non-positive definite ${\widehat{\mathbf{R}}}_k$ due to subtractive terms ${\mathbf{H}}_k{\mathbf{P}}_{k/k-1}{\mathbf{H}}_k^{\mathrm{T}}$, causing numerical divergence. Conversely, RAE leverages residuals ${\mathbf{\eta }}_k$ that inherently incorporate posterior state corrections. By computing covariance via sliding-window sample statistics, RAE ensures ${\widehat{\mathbf{R}}}_k$ remains positive semi-definite, while the structure of Equation (47) avoids destabilizing subtractions. This is critical for gravity-aided navigation where measurement noise varies dynamically. Therefore, compared to the IAE method, which can cause divergence due to negative definiteness, the RAE method is more reliable in practice. The proposed modified adaptive filter is based on the RAE method.

The sample mean of the residual is defined as:

$${\overline{\mathbf{\eta }}}_k={\displaystyle \frac{1}{\mathrm{M}}}{\displaystyle \sum _{i=k-M+1}^k{\mathbf{\eta }}_i}$$

(45)

The covariance using a limited number of samples of the residual is calculated by

$${\displaystyle \sum {\mathbf{\eta }}_k}={\displaystyle \frac{1}{\mathrm{M}-1}}{\displaystyle \sum _{i=k-\mathrm{M}+1}^k\left({\mathbf{\eta }}_i-{\overline{\mathbf{\eta }}}_k\right){\left({\mathbf{\eta }}_i-{\overline{\mathbf{\eta }}}_k\right)}^{\mathrm{T}}}$$

(46)

where $M$ is the estimation window size, which is an adjustable parameter. Then the recurrence formula ${\overline{\mathbf{\eta }}}_k$ of ${\mathbf{\eta }}_k$ is calculated as

$${\overline{\mathbf{\eta }}}_k={\displaystyle \frac{\mathrm{M}-1}{\mathrm{M}}}{\overline{\mathbf{\eta }}}_{k-1}+{\displaystyle \frac{1}{\mathrm{M}}}{\mathbf{\eta }}_k$$

(47)

The measurement noise matrix can be accurately obtained through the following equation enables

$$\begin{array}{ll}{\widehat{\mathbf{R}}}_k& ={\displaystyle \frac{\mathrm{M}-1}{\mathrm{M}}}{\widehat{\mathbf{R}}}_{k-1}+{\displaystyle \frac{1}{\mathrm{M}-1}}{\displaystyle \sum _{i=k-M+1}^k\left({\mathbf{\eta }}_i-{\overline{\mathbf{\eta }}}_k\right){\left({\mathbf{\eta }}_i-{\overline{\mathbf{\eta }}}_k\right)}^{\mathrm{T}}}\\ & +{\displaystyle \frac{1}{\mathrm{M}}}\left[{\displaystyle \sum _{j=i}^L{\mathbf{\omega }}_i}\left(h\left({\mathbf{\gamma }}_{k/k-1}^{\left(i\right)\ast }\right)-{\widehat{\mathit{z}}}_k\right){\left(h\left({\mathbf{\gamma }}_{k/k-1}^{\left(i\right)\ast }\right)-{\widehat{\mathit{z}}}_k\right)}^{\mathrm{T}}\right]\end{array}$$

(48)

Therefore, based on the aforementioned adaptive strategy, the complete workflow of gravity matching is presented as follows:

Step 1: INS calculation is carried out to obtain $\left\{\left.{\mathbf{q}}_b^n,{\mathbf{v}}^n,{\mathbf{p}}^n\right\}\right.$ through gyro and accelerometer outputs $\left\{{\mathbf{\omega }}_{ib}^b,\right.\left.{\mathbf{f}}^b\right\}$.

Step 2: Deriving the nonlinear error model of INS in (9)–(11) using $\left\{\left.{\mathbf{q}}_b^n,{\mathbf{v}}^n,{\mathbf{p}}^n\right\}\right.$ and $\left\{{\mathbf{\omega }}_{ib}^b,\right.\left.{\mathbf{f}}^b\right\}$.

Step 3: Choosing the difference between $\Delta {\mathrm{g}}_r\left({\mathbf{p}}_r\right)$ obtained from gravimeter and $\Delta {\mathrm{g}}_t\left({\mathbf{p}}_{INS}\right)$ extracted from map as measurement ${\mathbf{Z}}_k$.

Step 4: Using the modified adaptive filter-based matching algorithm proposed in this paper for underwater gravity-aided navigation described in (14)–(40), (47), (48), the navigation parameters errors $\left\{\left.\mathbf{\delta }\mathbf{q},\mathbf{\delta }{\mathbf{v}}^n,\mathbf{\delta }{\mathbf{p}}^n\right\}\right.$ are estimated.

Step 5: The estimated navigation parameters $\left\{\left.{\widehat{\mathbf{q}}}_b^n,{\widehat{\mathbf{v}}}^n,{\widehat{\mathbf{p}}}^n\right\}\right.$ are derived from the INS parameters and navigation errors, as follows

$$\left\{\begin{array}{l}{\widehat{\mathbf{q}}}_b^n={\mathbf{q}}_b^n\otimes \mathbf{\delta }\mathbf{q}\\ {\widehat{\mathbf{v}}}^n={\mathbf{v}}^n-\mathbf{\delta }{\mathbf{v}}^n\\ {\widehat{\mathbf{p}}}^n={\mathbf{p}}^n-\mathbf{\delta }{\mathbf{p}}^n\end{array}\right.$$

(49)

Step 6: The corrected parameters $\left\{\left.{\widehat{\mathbf{q}}}_b^n,{\widehat{\mathbf{v}}}^n,{\widehat{\mathbf{p}}}^n\right\}\right.$ are fed back to INS and reset the state ${\widehat{\mathbf{x}}}_k\left(1:9\right)$ to $0_{9\times 1}$.

Remark 1. 

This paper presents an improved algorithm compared to the traditional gravity-matching algorithm, focusing on two key aspects: the gravity-aided model and the matching filter. In terms of the gravity-aided model, the conventional Sandia inertial matching algorithm relies on a linear error model, which introduces linearization errors that can reduce navigation accuracy. In contrast, this study adopts a quaternion-based nonlinear error model, enhancing the gravity-matching performance. Regarding the matching filter, we propose a modified adaptive filter that accurately estimates the measurement noise matrix for accuracy and stability improvement in dynamic environments. MA-TCQUE employs orthogonal basis sampling to ensure uniform coverage of the attitude space, avoiding information loss caused by redundant or unevenly distributed sigma points in UKF/USQUE. Its local sampling concentration enhances estimation accuracy in critical nonlinear regions (e.g., near singularities) through adaptive density adjustment. Furthermore, MA-TCQUE guarantees positive-definite covariance via mathematical constraints, addressing potential numerical instability in UKF/USQUE. These features collectively improve robustness, accuracy. The algorithm’s superiority is demonstrated in the next section.

Remark 2. 

Compared to conventional methods, the MC-TCQUE approach demonstrates significant advantages. While particle filters can handle strong nonlinearities, they incur prohibitively high computational costs, whereas our method achieves lower complexity. Factor graph optimization relies on posterior trajectory optimization requiring batch processing, resulting in computational complexity and poor real-time performance. An adaptive CKF necessitates online noise parameter tuning, while MA-TCQUE ensures inherent stability through orthogonal basis sampling and positive-definite covariance. These comparisons highlight MA-TCQUE’s comprehensive superiority in computational efficiency and robustness performance.

4. Experimental Study

This section presents simulation and field tests to verify the proposed method. The modified adaptive filter-based matching algorithm (MA-TCQUE) is compared with the gravity-matching algorithm based on the UKF [8], TCQUE-based filter, and Sage-Husa TCQUE adaptive filter (SH-TCQUE). Gravity anomaly data is sourced from radar altimeter-derived gravity data (S&S-V29.1).

4.1. Simulation Study

This section presents a 72 h simulation with straight-line and curved trajectories, using the gravity field map shown in Figure 3. The vehicle’s gravity anomaly is simulated by adding 3mGal Gaussian white noise to the reference trajectory. Figure 4 shows the gravity anomaly data and

Table 1. Simulation parameter setting.

Symbol	Characteristic
Initial position error	${0.1}^{\prime }$
Initial velocity error	$0.01 \mathrm{m}/\mathrm{s}$
Gyroscope drift bias	${0.003}^{°}/\mathrm{h}$
Gyroscope random walk	${0.0003}^{°}/\sqrt{\mathrm{h}}$
Gyroscope update rate	$200 \mathrm{Hz}$
Accelerometer drift bias	$20 \mu \mathrm{g}$
Accelerometer random walk	$2 \mu \mathrm{g}/\sqrt{\mathrm{Hz}}$
Accelerometer update rate	$200 \mathrm{Hz}$

illustrates some simulation parameter settings.

In addition, process noise covariance is corresponding to the inertial sensor measurement is set as ${\mathbf{Q}}_k=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left({\left[{0.003}^{°}/h\cdot {\mathbf{I}}_{3\times 1};20 \mu \mathrm{g}\cdot {\mathbf{I}}_{3\times 1}\right]}^2\right)$, the initial state is set as ${\widehat{\mathbf{x}}}_0={\mathbf{0}}_{15\times 1}$, the initial state covariance matrix is set as ${\mathbf{P}}_k=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left({\left[{0.1}^{°};{0.1}^{°};{0.3}^{°};1 \mathrm{m}/\mathrm{s}\cdot {\mathbf{I}}_{3\times 1};10 \mathrm{m}\cdot {\mathbf{I}}_{3\times 1};{0.003}^{°}/h\cdot {\mathbf{I}}_{3\times 1};100 \mu \mathrm{g}\cdot {\mathbf{I}}_{3\times 1}\right]}^2\right)$, and the initial measurement noise covariance matrix is set as ${\mathbf{R}}_k=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left({\left[0.00001 \mathrm{m}/\mathrm{s}\right]}^2\right)$.

Different gravity-matching algorithms yield varied performance across navigation parameters, influenced by specific maneuverability conditions. This section analyzes the effectiveness of these gravity-matching algorithms, focusing on accurate evaluations to address the complexities of the volatile underwater environment. The Kalman filter is employed for updating navigation parameters and covariance at each step. The covariance matrix represents the confidence level, precision, and error variation in parameter estimation, reflecting the reliability of each state component. The observability degree for each estimator can be described as follows:

$${\sigma }_{k\left(j\right)}=\sqrt{{\displaystyle \frac{{\widehat{\mathbf{P}}}_{0\left(j,j\right)}}{{\widehat{\mathbf{P}}}_{k\left(j,j\right)}}}}$$

(50)

where ${\sigma }_{k\left(j\right)}$ represents the observability degree of the j state component at the k moment. The square root of the variance of the squared state quantity at the initial moment reflects the estimation error of that state during the filtering process. When the variance at the k moment increases, it indicates a higher measure of uncertainty; conversely, a larger variance corresponds to a lower measure of confidence. The observability metric ${\sigma }_{k\left(j\right)}$ implicitly connects to Lie derivative observability theory for nonlinear systems. While ${\sigma }_{k\left(j\right)}$ quantifies the error variance reduction ratio (derived from covariance evolution ${\widehat{\mathbf{P}}}_{k\left(j,j\right)}$), it fundamentally reflects the local weak observability characterized by Lie derivatives. Specifically, the gravity anomaly measurement model and system dynamics model generate a Lie derivative-based observability matrix. The growth of ${\sigma }_{k\left(j\right)}$ during maneuvers aligns with improved rank under trajectory excitation, validating its theoretical grounding in nonlinear observability analysis.

This paper analyzes the observability degree based on covariance using four different gravity-matching algorithms. The observability degrees of yaw, latitude, and longitude for these algorithms are presented in Figure 5, Figure 6 and Figure 7.

The results indicate that the maneuvering of the carrier affects the observability degree, with turning maneuvers particularly enhancing observability. Among the four algorithms, MA-TCQUE exhibits the highest observability degree, demonstrating superior precision and stability, which underscores its effectiveness. As shown in Figure 5, MA-TCQUE achieves a higher yaw observability (0.116) than the UKF (0.022), demonstrating that transformed sampling (Equations (16)–(20)) enhances attitude estimation.

Table 2. Observability degree statistics based on covariance matrix for four algorithms.

Algorithm	UKF	TCQUE	SH-TCQUE	MA-TCQUE
Pitch	0.320	0.076	0.075	0.126
Roll	0.024	0.040	0.039	0.059
Yaw	0.022	0.040	0.039	0.116
East velocity	1.013	1.449	1.436	2.679
North velocity	1.104	1.153	1.438	2.658
Latitude	0.004	0.005	0.005	0.017
Longitude	0.001	0.002	0.002	0.006
x-axis gyroscope	3.087	3.137	2.957	25.340
y-axis gyroscope	4.644	4.325	4.439	27.894
z-axis gyroscope	4.564	4.142	4.226	27.728
x-axis accelerometer	1.029	1.071	1.016	1.180
y-axis accelerometer	1.036	1.067	1.023	1.215
z-axis accelerometer	1.092	1.053	1.045	1.503

presents a statistical analysis of the average observability degree for attitude, velocity, position, gyroscope, and accelerometer data.

As shown in Table 2, the observability degree of all navigation parameters for MA-TCQUE is the highest among the four filtering matching algorithms. A higher observability degree correlates with increased covariance of the relative measurements, leading to greater filter precision and improved stability. The RMS error of MA-TCQUE (0.21 n mile) is 78% lower than UKF (0.95 n mile,

Table 3. Simulation parameter setting error statistics (unit: n mile).

Algorithm	Max	Mean	STD	RMS
INS	7.24	4.36	2.34	4.96
UKF	2.26	0.81	0.49	0.95
TCQUE	2.21	0.67	0.51	0.84
SH-TCQUE	1.02	0.34	0.28	0.45
MA-TCQUE	0.64	0.18	0.11	0.21

), confirming the synergy between the nonlinear model and adaptive noise estimation. Thus, the effectiveness of the improved adaptive filtering matching algorithm is validated by the observability degree observed in the simulation study.

Then a total of 10 Monte Carlo runs are carried out for the four algorithms. The latitude, longitude, and position error by four algorithms are shown in Figure 8, Figure 9 and Figure 10. It is shown that the MA-TCQUE has the best performance among the 10 Monte Carlo runs.

Figure 10, Figure 11 and Figure 12 compare the average latitude, longitude, and horizontal position errors from 10 Monte Carlo runs for 4 matching algorithms against the INS results. All algorithms effectively correct INS position errors, with MA-TCQUE showing the best performance. The TCQUE algorithm outperforms the traditional UKF, due to the advantages of the quaternion-based nonlinear error model over the linear Euler angle model. Additionally, the MA-TCQUE algorithm surpasses SH-TCQUE, demonstrating improved accuracy and filtering stability.

The accuracy of the different matching algorithms is analyzed using maximum (Max), mean (Mean), standard deviation (STD), and root mean square error (RMS), as shown in Table 3. The results indicate that the Max, Mean, STD, and RMS values for TCQUE are 2.21, 0.67, 0.51, and 0.84, representing improvements of 2.2%, 17.2%, 4.1%, and 11.6% over the UKF algorithm, respectively. Additionally, the Max, Mean, STD, and RMS values for TCQUE are 0.64, 0.18, 0.11, and 0.21, showing enhancements of 37.3%, 47.1%, 60.7%, and 53.3% compared to SH-TCQUE. Furthermore, the Max, Mean, STD, and RMS values for MA-TCQUE are the lowest among all tested algorithms. These findings clearly demonstrate that the modified adaptive filter-based matching algorithm utilizing transformed cubature quaternion estimation for underwater gravity-aided navigation offers the best performance in terms of filtering accuracy and stability.

4.2. Field Test

To evaluate the proposed method, a shipboard navigation experiment was conducted in the western Pacific Ocean. The marine gravimeter setup, shown in Figure 13, has an accuracy of 1mGal. The IMU, the primary gravimeter, includes three laser gyroscopes and three quartz accelerometers, providing 200 Hz gyro and accelerometer data. IMU specifications are shown in

Table 4. Specifications of equipment.

Algorithm Sensor	Max Characteristic	Value
Accelerometer	Bias	${0.002}^{°}/\mathrm{h}$
	Random walk	${0.0002}^{°}/\sqrt{\mathrm{h}}$
	Update rate	$200 \mathrm{Hz}$
SH-TCQUE	Bias	$20 \mu \mathrm{g}$
	Random walk	$20 \mu \mathrm{g}$
	Update rate	$200 \mathrm{Hz}$

. The GNSS system delivers velocity with 0.1 m/s precision and position accuracy of 10 m, updating at 1 Hz. SINS-GNSS integration results serve as reference data.

The 24 h experiment took place in the western Pacific Ocean, with the trajectory shown in Figure 14 (red mark for start, blue for end). The ship, heading southeast, is shown in Figure 15, while Figure 16 presents the gravity anomaly data for the area.

Figure 17, Figure 18 and Figure 19 show the latitude, longitude, and horizontal position errors for the four algorithms, while

Table 5. Error statistics (unit n mile).

Algorithm	Max	Mean	STD	RMS
INS	1.03	0.42	0.25	0.49
UKF	0.37	0.18	0.09	0.20
TCQUE	0.31	0.14	0.08	0.17
SH-TCQUE	0.41	0.13	0.07	0.15
MA-TCQUE	0.31	0.08	0.06	0.12

lists the Max, Mean, STD, and RMS values. All methods effectively suppress INS error divergence, but the proposed method achieves superior accuracy. Although the velocity is 0 m/s and the ship is stationary, with no change in gravity anomaly, position errors increase for all algorithms. However, due to adaptive adjustment, the MA-TCQUE outperforms the others.

Statistical analysis reveals that TCQUE improves by 16.2% (Max), 22.2% (Mean), 11.1% (STD), and 15.0% (RMS) over the UKF. Additionally, MA-TCQUE achieves enhancements of 24.4% (Max), 38.5% (Mean), 14.3% (STD), and 20.0% (RMS) compared to SH-TCQUE. Overall, MA-TCQUE exhibits the best comprehensive performance in the experiments, while TCQUE outperforms the UKF. These results verify the superiority of the proposed algorithm.

To comprehensively evaluate performance, we added computational time statistics in

Table 6. Computational complexity comparison.

Algorithm	Theoretical FLOPs	Computational Time (s)
UKF	O(2n3)	3144
TCQUE	O(4n3)	4429.3
SH-TCQUE	O(4n3 + mn2)	4584.1
MA-TCQUE	O(4n3 + kmn2)	4518.9

Note: n = 12 (states), m = 1 (measurement), k = 20 (sliding window).

. The results shows that TCQUE, SH-TCQUE, and MA-TCQUE algorithms exhibit increased processing time compared to UKF, primarily due to the orthogonal transformation (TC transformation) embedded in all three variants. Crucially, while the time costs among these advanced algorithms are comparable (differing by <5%), MA-TCQUE achieves significant accuracy gains—reducing RMS position error by 77.9% versus UKF in simulations (0.21 vs. 0.95 n mile) and 40% in field tests (0.12 vs. 0.20 n mile). This demonstrates that the computational overhead is justified by MA-TCQUE’s superior precision and stability, especially in dynamic underwater environments where traditional methods falter.

5. Conclusions

Gravity matching plays a crucial role in gravity-aided inertial navigation systems, providing essential data for enhancing navigation accuracy. Traditional methods, such as the Sandia inertial matching algorithm, rely on a linear error model. However, this approach introduces linearization errors, which can negatively impact navigation precision. To overcome these limitations, we propose a novel algorithm known as the modified adaptive transformed cubature quaternion estimation (MA-TCQUE). This new method is tailored for a nonlinear error model, significantly improving accuracy in gravity-aided navigation. In addition to enhancing precision, the MA-TCQUE algorithm also offers improved filtering stability, particularly in complex and dynamic environments, where traditional methods may struggle. The effectiveness of the proposed algorithm is validated by simulation and experimental results, showing advantages over existing techniques. The MA-TCQUE algorithm represents a significant advancement in gravity-matching technology, enhancing INS performance in various applications.