2025
A Low-Order Gravity Disturbance Compensation Algorithm Based on Carrier Motion Constraints
Yu Wang, Wenzhe Zhang, Shengwu Zhao, Zhihong Deng# (# corresponding author)
IEEE Sensors Journal (IEEE JSEN) 2025 Journal
Gravity disturbance compensation technology is an important means to further enhance the positioning accuracy of high-precision inertial navigation systems (INSs). In response to the challenges faced by traditional gravity disturbance acquisition methods, which are computationally complex and time-consuming, this article proposes a gravity disturbance calculation and compensation method based on carrier motion constraints. First, using velocity information as a constraint, a conversion model is constructed for the low-frequency signal of gravity disturbance to calculate the low-order spherical harmonic model. This model significantly reduces the time cost required for the gravity disturbance model computation. Second, addressing the misalignment between the actual navigation coordinate system and the ideal navigation coordinate system caused by gravity disturbances, a coordinate system correction algorithm based on the direction cosine matrix of disturbances is proposed. This algorithm enhances the positioning accuracy and reliability of high-precision INSs. Experimental results show that the proposed low-order gravity disturbance compensation algorithm based on carrier motion constraints improves the positioning accuracy by 27.89% compared to traditional algorithms while reducing computation time by 64.84%. This meets the real-time positioning requirements for long-distance navigation conditions, especially suited for UUVs, AUVs, and submarine platforms with limited computational resources, as it optimizes processing efficiency while maintaining high accuracy.
A Low-Order Gravity Disturbance Compensation Algorithm Based on Carrier Motion Constraints
Yu Wang, Wenzhe Zhang, Shengwu Zhao, Zhihong Deng# (# corresponding author)
IEEE Sensors Journal (IEEE JSEN) 2025 Journal
Gravity disturbance compensation technology is an important means to further enhance the positioning accuracy of high-precision inertial navigation systems (INSs). In response to the challenges faced by traditional gravity disturbance acquisition methods, which are computationally complex and time-consuming, this article proposes a gravity disturbance calculation and compensation method based on carrier motion constraints. First, using velocity information as a constraint, a conversion model is constructed for the low-frequency signal of gravity disturbance to calculate the low-order spherical harmonic model. This model significantly reduces the time cost required for the gravity disturbance model computation. Second, addressing the misalignment between the actual navigation coordinate system and the ideal navigation coordinate system caused by gravity disturbances, a coordinate system correction algorithm based on the direction cosine matrix of disturbances is proposed. This algorithm enhances the positioning accuracy and reliability of high-precision INSs. Experimental results show that the proposed low-order gravity disturbance compensation algorithm based on carrier motion constraints improves the positioning accuracy by 27.89% compared to traditional algorithms while reducing computation time by 64.84%. This meets the real-time positioning requirements for long-distance navigation conditions, especially suited for UUVs, AUVs, and submarine platforms with limited computational resources, as it optimizes processing efficiency while maintaining high accuracy.
Adaptive Point Mass Filter and Its Application in Terrain Matching Navigation
Shengwu Zhao, Zhihong Deng#, Wenzhe Zhang, Yu Wang (# corresponding author)
IEEE Transactions on Instrumentation and Measurement (IEEE TIM) 2025 Journal
Terrain matching algorithm is the core part of terrain-aided inertial navigation system (INS), and point mass filter (PMF) is often used as a terrain matching algorithm. In PMF, the construction method and calculation amount of the point mass set are important parts which affect the accuracy. To comprehensively consider the construction of point mass set and calculation amount, this article proposes an adaptive construction method of point mass set based on probability distribution. According to the characteristics of the probability distribution to be approximated, the method transforms the preset point mass set to obtain point mass set of the current moment. Aiming at the problem of calculation error, which is caused by the process noise intensity being smaller than the resolution of the point mass set, a local convolution method is proposed in this article, in which the convolution result is obtained by subdividing the local area near the point mass set. Finally, the framework of the proposed adaptive PMF (APMF) in terrain matching is given. Numerical simulations show the superiority of the proposed APMF, and the experiment shows the effectiveness of the proposed method in terrain-aided INS.
Adaptive Point Mass Filter and Its Application in Terrain Matching Navigation
Shengwu Zhao, Zhihong Deng#, Wenzhe Zhang, Yu Wang (# corresponding author)
IEEE Transactions on Instrumentation and Measurement (IEEE TIM) 2025 Journal
Terrain matching algorithm is the core part of terrain-aided inertial navigation system (INS), and point mass filter (PMF) is often used as a terrain matching algorithm. In PMF, the construction method and calculation amount of the point mass set are important parts which affect the accuracy. To comprehensively consider the construction of point mass set and calculation amount, this article proposes an adaptive construction method of point mass set based on probability distribution. According to the characteristics of the probability distribution to be approximated, the method transforms the preset point mass set to obtain point mass set of the current moment. Aiming at the problem of calculation error, which is caused by the process noise intensity being smaller than the resolution of the point mass set, a local convolution method is proposed in this article, in which the convolution result is obtained by subdividing the local area near the point mass set. Finally, the framework of the proposed adaptive PMF (APMF) in terrain matching is given. Numerical simulations show the superiority of the proposed APMF, and the experiment shows the effectiveness of the proposed method in terrain-aided INS.
2024
A Gravity-Aided Navigation Matching Algorithm Based on Triangulation
Yu Wang, Zhihong Deng#, Peiyuan Zhang, Bo Wang, Shengwu Zhao (# corresponding author)
IEEE Sensors Journal (IEEE JSEN) 2024 Journal
Gravity-aided inertial navigation system (GAINS) is one of the key technologies in underwater navigation. The traditional gravity background field is usually composed of gravity anomaly values measured by gravity sensors, which are in the form of a grid and hide the rich local features within the local field. This article uses the Mercator projection and Delaunay triangulation method to convert the traditional gravity field structure into a triangular model. This new gravity triangulation model (GTM) divides the entire field into numerous small triangles, each representing more localized gravity spatial characteristics. Then a new gravity-matching algorithm based on the computational geometry “plane-line-point” model is proposed which can reduce the error of traditional filtering algorithms in processing nonlinear features. The optimal position estimation of the underwater vehicle is obtained through rough matching of triangular surfaces, secondary matching of intersection lines, precise matching of track points, and spatial affine transformation. The sea experiments demonstrate that after working for about 6 h, the mean position error of the proposed algorithm is 915.85 m, and the standard deviation of the position error is 488.3542 m, reaching 0.26 grids, which is 69.38% higher than the accuracy of the inertial navigation system (INS) and 56.18% higher than the existing iterative closest contour point (ICCP) algorithm, which effectively improves the positioning accuracy of underwater navigation.
A Gravity-Aided Navigation Matching Algorithm Based on Triangulation
Yu Wang, Zhihong Deng#, Peiyuan Zhang, Bo Wang, Shengwu Zhao (# corresponding author)
IEEE Sensors Journal (IEEE JSEN) 2024 Journal
Gravity-aided inertial navigation system (GAINS) is one of the key technologies in underwater navigation. The traditional gravity background field is usually composed of gravity anomaly values measured by gravity sensors, which are in the form of a grid and hide the rich local features within the local field. This article uses the Mercator projection and Delaunay triangulation method to convert the traditional gravity field structure into a triangular model. This new gravity triangulation model (GTM) divides the entire field into numerous small triangles, each representing more localized gravity spatial characteristics. Then a new gravity-matching algorithm based on the computational geometry “plane-line-point” model is proposed which can reduce the error of traditional filtering algorithms in processing nonlinear features. The optimal position estimation of the underwater vehicle is obtained through rough matching of triangular surfaces, secondary matching of intersection lines, precise matching of track points, and spatial affine transformation. The sea experiments demonstrate that after working for about 6 h, the mean position error of the proposed algorithm is 915.85 m, and the standard deviation of the position error is 488.3542 m, reaching 0.26 grids, which is 69.38% higher than the accuracy of the inertial navigation system (INS) and 56.18% higher than the existing iterative closest contour point (ICCP) algorithm, which effectively improves the positioning accuracy of underwater navigation.
The Analysis of Influencing Factors on Geophysical Field Matching
Shengwu Zhao, Wenzhe Zhang, Yu Wang, Zhihong Deng# (# corresponding author)
International Conference on Guidance, Navigation and Control (ICGNC) 2024 Conference
The analysis of the factors affecting the accuracy of geophysical field matching can effectively provide guidance for the design and selection of matching algorithms. This paper mainly analyzes the influence of gravity field background map, gravity measurement error and inertial navigation position error on the matching. Firstly, the influence of the resolution of the gravity field background map, the accuracy of the grid points and the interpolation area on the reading value was studied. Secondly, taking the TERCOM algorithm as an example, the covariance matrix corresponding to the matching error is given. Simulation results show the effectiveness of the proposed theory.
The Analysis of Influencing Factors on Geophysical Field Matching
Shengwu Zhao, Wenzhe Zhang, Yu Wang, Zhihong Deng# (# corresponding author)
International Conference on Guidance, Navigation and Control (ICGNC) 2024 Conference
The analysis of the factors affecting the accuracy of geophysical field matching can effectively provide guidance for the design and selection of matching algorithms. This paper mainly analyzes the influence of gravity field background map, gravity measurement error and inertial navigation position error on the matching. Firstly, the influence of the resolution of the gravity field background map, the accuracy of the grid points and the interpolation area on the reading value was studied. Secondly, taking the TERCOM algorithm as an example, the covariance matrix corresponding to the matching error is given. Simulation results show the effectiveness of the proposed theory.
Terrain Matching Algorithm Based on Trajectory Reconstruction and Correlation Analysis of Sliding Measurement Sequence
Shengwu Zhao, Zhihong Deng#, Qingzhe Wang, Wenzhe Zhang, Xun Gong (# corresponding author)
IEEE/ASME Transactions on Mechatronics (IEEE TMECH) 2024 Journal
Single-point matching algorithm (point mass filter or particle filter) only uses the current time measurement to calculate the likelihood, which is prone to pseudopeak and false peak. In order to solve the problem, this article introduces the sequence correlation analysis into the single point matching algorithm, and uses the sliding measurement sequence to estimate recursively. First, a position sequence estimation method based on trajectory reconstruction is proposed, which calculates the new position sequence by the relationship between INS displacement and heading angle, instead of the direct translation of INS trajectory method in traditional algorithms. After that, the likelihood of the candidate point is calculated by the correlation analysis method using the corresponding sliding measurement sequence at the current time, and a more accurate position estimation is obtained after the measurement update. Simulation and experiments show that the position sequence obtained by the proposed method based on trajectory reconstruction is more accurate than that obtained by the direct translation inertial navigation method. Compared with only using single time measurement information, the likelihood calculation method based on correlation analysis of sliding measurement sequence can significantly reduce pseudopeak and false peak, and the positioning accuracy of terrain matching is improved.
Terrain Matching Algorithm Based on Trajectory Reconstruction and Correlation Analysis of Sliding Measurement Sequence
Shengwu Zhao, Zhihong Deng#, Qingzhe Wang, Wenzhe Zhang, Xun Gong (# corresponding author)
IEEE/ASME Transactions on Mechatronics (IEEE TMECH) 2024 Journal
Single-point matching algorithm (point mass filter or particle filter) only uses the current time measurement to calculate the likelihood, which is prone to pseudopeak and false peak. In order to solve the problem, this article introduces the sequence correlation analysis into the single point matching algorithm, and uses the sliding measurement sequence to estimate recursively. First, a position sequence estimation method based on trajectory reconstruction is proposed, which calculates the new position sequence by the relationship between INS displacement and heading angle, instead of the direct translation of INS trajectory method in traditional algorithms. After that, the likelihood of the candidate point is calculated by the correlation analysis method using the corresponding sliding measurement sequence at the current time, and a more accurate position estimation is obtained after the measurement update. Simulation and experiments show that the position sequence obtained by the proposed method based on trajectory reconstruction is more accurate than that obtained by the direct translation inertial navigation method. Compared with only using single time measurement information, the likelihood calculation method based on correlation analysis of sliding measurement sequence can significantly reduce pseudopeak and false peak, and the positioning accuracy of terrain matching is improved.
2023
Gravity Matching Algorithm Based on Backtracking for Small Range Adaptation Area
Shengwu Zhao, Xuan Xiao, Xuan Pang, Yu Wang, Zhihong Deng# (# corresponding author)
IEEE Transactions on Instrumentation and Measurement (IEEE TIM) 2023 Journal
The existing gravity matching algorithms are affected either by gravity measurement error or by the initial position of inertial navigation system (INS). Filter algorithms can solve the problem under the condition of enough measurement data. However, the range of most adaptation areas is small. Due to the long matching period, the available measurement data may be not enough to make the filter converge. Aiming to obtain high-precision position information in the small range adaptation area, backtracking strategies that combine the filter algorithm are first proposed in this article. Next, the observability of gravity-aided navigation system is also analyzed based on graph analysis. Furthermore, the reverse error equations are obtained by analogy corresponding to the reverse solution, and the relationship between forward navigation and reverse navigation is also given. The results of simulation and marine experiment show that the proposed algorithm is superior to the existing gravity matching algorithms, and has a high positioning accuracy in the small range adaptation area.
Gravity Matching Algorithm Based on Backtracking for Small Range Adaptation Area
Shengwu Zhao, Xuan Xiao, Xuan Pang, Yu Wang, Zhihong Deng# (# corresponding author)
IEEE Transactions on Instrumentation and Measurement (IEEE TIM) 2023 Journal
The existing gravity matching algorithms are affected either by gravity measurement error or by the initial position of inertial navigation system (INS). Filter algorithms can solve the problem under the condition of enough measurement data. However, the range of most adaptation areas is small. Due to the long matching period, the available measurement data may be not enough to make the filter converge. Aiming to obtain high-precision position information in the small range adaptation area, backtracking strategies that combine the filter algorithm are first proposed in this article. Next, the observability of gravity-aided navigation system is also analyzed based on graph analysis. Furthermore, the reverse error equations are obtained by analogy corresponding to the reverse solution, and the relationship between forward navigation and reverse navigation is also given. The results of simulation and marine experiment show that the proposed algorithm is superior to the existing gravity matching algorithms, and has a high positioning accuracy in the small range adaptation area.
2022
Gravity Matching Algorithm Based on Correlation Filter
Shengwu Zhao, Xuan Xiao#, Zhihong Deng, Lei Shi (# corresponding author)
IEEE Sensors Journal (IEEE JSEN) 2022 Journal
The gravity-matching algorithm is one of the key technologies in the gravity-aided inertial navigation system (GAINS). The matching accuracy determines the correction accuracy of the inertial navigation system (INS). However, the initial position error of the INS and gravity measurement error lead to a decrease in matching accuracy. In this article, the characteristics of gravity measurement error and inertial navigation information are made full use of to reduce the impact on matching. A gravity-matching algorithm based on a correlation filter (CF) is proposed, which includes preprocessing, CF, and mismatch detection. Meanwhile, the shape of the INS trajectory is used as a constraint to reduce the mismatch caused by the measurement error to improve the matching accuracy. Moreover, two matching strategies are given, including normal matching and sliding window matching. Experimental results show that the proposed method can effectively improve the matching accuracy under initial position error and gravity measurement error.
Gravity Matching Algorithm Based on Correlation Filter
Shengwu Zhao, Xuan Xiao#, Zhihong Deng, Lei Shi (# corresponding author)
IEEE Sensors Journal (IEEE JSEN) 2022 Journal
The gravity-matching algorithm is one of the key technologies in the gravity-aided inertial navigation system (GAINS). The matching accuracy determines the correction accuracy of the inertial navigation system (INS). However, the initial position error of the INS and gravity measurement error lead to a decrease in matching accuracy. In this article, the characteristics of gravity measurement error and inertial navigation information are made full use of to reduce the impact on matching. A gravity-matching algorithm based on a correlation filter (CF) is proposed, which includes preprocessing, CF, and mismatch detection. Meanwhile, the shape of the INS trajectory is used as a constraint to reduce the mismatch caused by the measurement error to improve the matching accuracy. Moreover, two matching strategies are given, including normal matching and sliding window matching. Experimental results show that the proposed method can effectively improve the matching accuracy under initial position error and gravity measurement error.
An Improved Particle Filter Based on Gravity Measurement Feature in Gravity-Aided Inertial Navigation System
Shengwu Zhao, Xuan Xiao#, Yu Wang, Zhihong Deng (# corresponding author)
IEEE Sensors Journal (IEEE JSEN) 2022 Journal
The existing gravity matching algorithms are affected by the initial position error of the inertial navigation system (INS), the gravity measurement error, and the similarity of the gravity background map. Aiming at the above problems, an improved particle filter based on the gravity measurement feature (IPFBGMF) is proposed in this article. In the IPFBGMF, both the value and change characteristic of gravity measurements are considered, and a novel position acquisition method based on the gravity measurement feature is proposed, which can reduce the influence of the initial position error of INS. In addition, a new concept called direction measurement using the heading angle of INS is proposed to optimize the weight of particles in the PF. The PF with direction measurement can reduce the influence of the gravity measurement error and the similarity of the gravity background map. Furthermore, the robustness of the improved PF with the precise position is proven. Finally, a navigation strategy is designed to apply the proposed algorithms. Simulations show that IPFBGMF has the highest positioning accuracy compared with the traditional gravity matching algorithms.
An Improved Particle Filter Based on Gravity Measurement Feature in Gravity-Aided Inertial Navigation System
Shengwu Zhao, Xuan Xiao#, Yu Wang, Zhihong Deng (# corresponding author)
IEEE Sensors Journal (IEEE JSEN) 2022 Journal
The existing gravity matching algorithms are affected by the initial position error of the inertial navigation system (INS), the gravity measurement error, and the similarity of the gravity background map. Aiming at the above problems, an improved particle filter based on the gravity measurement feature (IPFBGMF) is proposed in this article. In the IPFBGMF, both the value and change characteristic of gravity measurements are considered, and a novel position acquisition method based on the gravity measurement feature is proposed, which can reduce the influence of the initial position error of INS. In addition, a new concept called direction measurement using the heading angle of INS is proposed to optimize the weight of particles in the PF. The PF with direction measurement can reduce the influence of the gravity measurement error and the similarity of the gravity background map. Furthermore, the robustness of the improved PF with the precise position is proven. Finally, a navigation strategy is designed to apply the proposed algorithms. Simulations show that IPFBGMF has the highest positioning accuracy compared with the traditional gravity matching algorithms.
2021
Global Dynamic Path-planning Algorithm in Gravity-aided Inertial Navigation System
Shengwu Zhao, Lei Shi, Wenzhe Zhang, Zhihong Deng# (# corresponding author)
IET Signal Processing (IET SP) 2021 Journal
Signals from gyros and accelerometers in an inertial navigation system (INS) can be processed to obtain navigation information while its errors accumulate with time. A path with long-term characteristics is more likely to encounter unpredictable dynamic conditions. To deal with such problems, this work improves the global static path-planning algorithm based on the A* algorithm, and the local dynamic path-planning algorithm based on dynamic window approach (DWA). In the global planning algorithm, the gravity index and the heading index are incorporated into the cost function of the A* algorithm to consider the mismatches and the deflection of the path. Furthermore, the search method of the A* algorithm is also improved. In the local planning algorithm, the motion model of the DWA algorithm is improved to a high-precision model and expressed in a geographic coordinate system. Finally, the improved global static planning and local dynamic planning are combined as the global dynamic path-planning algorithm of an underwater vehicle. The simulation results show that the global dynamic path-planning algorithm proposed can realise obstacle-free navigation of the underwater vehicle.
Global Dynamic Path-planning Algorithm in Gravity-aided Inertial Navigation System
Shengwu Zhao, Lei Shi, Wenzhe Zhang, Zhihong Deng# (# corresponding author)
IET Signal Processing (IET SP) 2021 Journal
Signals from gyros and accelerometers in an inertial navigation system (INS) can be processed to obtain navigation information while its errors accumulate with time. A path with long-term characteristics is more likely to encounter unpredictable dynamic conditions. To deal with such problems, this work improves the global static path-planning algorithm based on the A* algorithm, and the local dynamic path-planning algorithm based on dynamic window approach (DWA). In the global planning algorithm, the gravity index and the heading index are incorporated into the cost function of the A* algorithm to consider the mismatches and the deflection of the path. Furthermore, the search method of the A* algorithm is also improved. In the local planning algorithm, the motion model of the DWA algorithm is improved to a high-precision model and expressed in a geographic coordinate system. Finally, the improved global static planning and local dynamic planning are combined as the global dynamic path-planning algorithm of an underwater vehicle. The simulation results show that the global dynamic path-planning algorithm proposed can realise obstacle-free navigation of the underwater vehicle.
An Improved ICCP Gravity Matching Algorithm Based on Mahalanobis Distance
Yu Wang, Zhihong Deng#, Wenzhe Zhang, Shengwu Zhao (# corresponding author)
2021 40th Chinese Control Conference (CCC) 2021 Conference
Consider the problems of low accuracy and poor real-time performance in traditional ICCP gravity matching algorithm, this paper proposes an improved ICCP gravity matching algorithm based on Mahalanobis distance. Mahalanobis distance, instead of Euclidean distance, is applied to improve the objective function of rigid transformation iteration in traditional ICCP algorithm, which can eliminate the interference of variable correlation. At the same time, a new searching strategy based on strip search area is proposed. With the current sampling point as the center, we use the gravimeter measurement error variance to determine the width of the strip region along the normal direction of the contour, and use the Mahalanobis distance objective function value obtained in the iterative process as the length of the strip area along the direction of the contour. Therefore, this searching strategy can gradually narrow the search range of the closest contour point in the ICCP algorithm, and reduce the calculation amount of the algorithm. The simulation results show that: Compared with the traditional ICCP algorithm, the mean value of the position error of the improved algorithm is reduced by 26.89%, and the standard deviation is reduced by 58.53%, which greatly improves the accuracy of the gravity matching algorithm. And the average running time of the improved algorithm is also 42.1% shorter than the traditional ICCP algorithm, so the real-time performance is improved. The improved ICCP gravity matching algorithm based on Mahalanobis distance reduces the running time of the algorithm, improves real-time performance and positioning accuracy, and further improves the practicability of gravity matching for underwater vehicles.
An Improved ICCP Gravity Matching Algorithm Based on Mahalanobis Distance
Yu Wang, Zhihong Deng#, Wenzhe Zhang, Shengwu Zhao (# corresponding author)
2021 40th Chinese Control Conference (CCC) 2021 Conference
Consider the problems of low accuracy and poor real-time performance in traditional ICCP gravity matching algorithm, this paper proposes an improved ICCP gravity matching algorithm based on Mahalanobis distance. Mahalanobis distance, instead of Euclidean distance, is applied to improve the objective function of rigid transformation iteration in traditional ICCP algorithm, which can eliminate the interference of variable correlation. At the same time, a new searching strategy based on strip search area is proposed. With the current sampling point as the center, we use the gravimeter measurement error variance to determine the width of the strip region along the normal direction of the contour, and use the Mahalanobis distance objective function value obtained in the iterative process as the length of the strip area along the direction of the contour. Therefore, this searching strategy can gradually narrow the search range of the closest contour point in the ICCP algorithm, and reduce the calculation amount of the algorithm. The simulation results show that: Compared with the traditional ICCP algorithm, the mean value of the position error of the improved algorithm is reduced by 26.89%, and the standard deviation is reduced by 58.53%, which greatly improves the accuracy of the gravity matching algorithm. And the average running time of the improved algorithm is also 42.1% shorter than the traditional ICCP algorithm, so the real-time performance is improved. The improved ICCP gravity matching algorithm based on Mahalanobis distance reduces the running time of the algorithm, improves real-time performance and positioning accuracy, and further improves the practicability of gravity matching for underwater vehicles.