INS/gravity gradient aided navigation based on gravitation field particle filter

Fanming Liu; Fangming Li; Xin Jing

doi:10.1515/phys-2019-0073

Article Open Access

INS/gravity gradient aided navigation based on gravitation field particle filter

Fanming Liu , Fangming Li and Xin Jing

Published/Copyright: December 30, 2019

Published by

Become an author with De Gruyter Brill

Submit Manuscript Author Information Explore this Subject

From the journal Open Physics Volume 17 Issue 1

Abstract

Swarm intelligence method is an effective way to improve the particle degradation and sample depletion of the traditional particle filter. This paper proposes a particle filer based on the gravitation field algorithm (GF-PF), and the gravitation field algorithm is introduced into the resampling process to improve particle degradation and sample depletion. The gravitation field algorithm simulates the solar nebular disk model, and introduces the virtual central attractive force and virtual rotation repulsion force between particles. The particles are moves rapidly to the high-likelihood region under action of the virtual central attractive force. The virtual rotation repulsion force makes the particles keep a certain distance from each other. These operations improve estimation performance, avoid overlapping of particles and maintain the diversity of particles. The proposed method is applied into INS/gravity gradient aided navigation, by combining the sea experimental data of an inertial navigation system. Compared with the particle swarm optimization particle filter(PSO-PF) and artificial physics optimized particle filter (APO-PF), the GF-PF has higher position estimate accuracy and faster convergence speed with the same experimental conditions.

Keywords: gravitation field; solar nebular disk mode; particle filter; gravity gradient; virtual forces

PACS: 02.30.Yy; 02.70.Uu; 11.10.Lm; 91.50.-r; 91.10.-v

1 Introduction

The inertial navigation system (INS) is the mainly navigation system of underwater vehicle. The positioning error of the INS is accumulated and diverged over time, and other positioning methods are needed to correct systematic positioning errors periodically [1, 2, 3, 4, 5, 6]. When GPS, radio location and terrain aided navigation are used to correct the positioning error of INS on underwater vehicle, there are disadvantages that require underwater vehicle needs to transmit or receive signals, which destroy the concealment of the underwater vehicle. Gravity gradient aided navigation has the ability to correct INS positioning error [7]. While gravity gradiometer obtaining the gravity gradient data, the underwater vehicle need not transmit or receive signals. The underwater vehicle can be autonomous positioning even in the special case of GPS or radio location failure.

The principle of INS/gravity gradient aided navigation is to use matching method to achieve the optimal approximation from the INS indicated position to the real position of the underwater vehicle [8, 9]. Matching algorithm is one of the core technologies of gravity/gravity gradient aided navigation system. Numerous scholars have applied TERCOM algorithm [10], ICCP algorithm [11, 12] and SITAN algorithm [13] to gravity/gravity gradient aided navigation, and achieved well results. However, these algorithms have some shortcomings and need to be strengthened for higher positioning accuracy. If the initial positioning error exceeds the allowable range, the matching of ICCP algorithm will fail [14]. It is difficult to establish an accurate model for SITAN algorithm [15]. And the real-time performance and computational complexity of TERCOM algorithm need to be further improved [16]. Particle filter is commonly used in non-linear and non-Gaussian systems such as location tracking and fault diagnosis [17, 18, 19]. Traditional particle filter methods have defects such as particle degradation and sample depletion. In recent years, some scholars have combined swarm intelligence method with particle filter to improve these problems. For example, artificial physical optimization particle filter [20], chicken swarm optimization particle filter [21], firefly algorithm optimization particle filter [22], bat algorithm optimization particle filter

[23], etc. Swarm intelligence method regards particles as individuals in a biological population. It makes the distribution of particles more reasonable by simulating the movement of biological clusters. It does not involve the direct abandonment of low-weight particles,which can effectively solve the problems of particle degradation and simple exhaustion. Some scholars applied particle filter algorithm to gravity/gravity gradient aided navigation, and obtain good positioning results. LIU [20, 24] proposes an artificial optimization particle filter algorithm, and applied it to gravity / gravity gradient aided navigation system. It gets a positioning accuracy of 133.6m.WANG [25] proposes a PF-based matching algorithm with gravity sample vector, and the CEP is 0.42^′. LIU [26] proposes a self-adaptive artificial physics optimized particle filter, and the positioning accuracy is 86.9m.

In this paper, a gravitation field particle filter algorithm (GF-PF) is proposed. Through the interactive virtual force between particles, particles aggregate into the high-likelihood region, the posterior distribution of state is optimized, the sample dilution problem is solved, and the accuracy of the particle filter is improved. The method is applied to INS/gravity gradient aided navigation system. Compared with particle swarm optimization particle filter(PSO-PF) [27] and artificial physics optimized particle filter (APO-PF), the simulation results show that the method proposed in this paper can obtain higher positioning accuracy and faster convergence speed under different test conditions. It can be used to correct the position error of the INS.

The rest of the paper is organized as follow: In Section 2, the principle of INS/gravity gradient aided navigation is explained; in Section 3, the particle filter based on gravitation field algorithm is described; in Section 4, the particle filter based on gravitation field algorithm model is applied to INS/gravity gradient aided navigation and compared with PSO-PF and APO-PF to verify the performance of the method proposed in this paper under different test conditions. Finally, the Section 5 is a summary of the full paper.

2 INS/Gravity gradient aided navigation

2.1 Principle of INS/gravity gradient aided navigation

The gravity gradient is the full spatial derivative of gravity acceleration. Due to the gravity gradient symmetry and the Poisson equation, there are total five independent tensors of gravity gradient components in different directions, which provide accurate and reliable reference maps for gravity gradient aided navigation [28]. INS/gravity gradient aided navigation is a navigation method that matches the real-time measured gravity gradient data with the prestored high-precision gravity gradient maps which can get underwater vehicle position information real-time. The system structure is shown in Figure 1.

Figure 1

Structure for INS/gravity aided navigation.

During the underwater vehicle navigation, the matching algorithm searches the gravity gradient data from the gravity gradient maps according to the INS indicated position, and matches with the data from gravity gradiometer to get the accurate position of the underwater vehicle. The position is used to revise the positioning error of the INS.

2.2 INS error equation

The INS error equation is usually used as part of integrated navigation model, which is the basis of error estimation and correction of the INS. The INS error equation includes position error equation, velocity error equation and attitude angle error equation. The position error equation is as follow.

(1) δ φ ˙ = 1 R M δ V y . δ λ ˙ = V x R N tan ⁡ φ sec ⁡ φ δ φ + sec φ R N δ V x .

Where, δφ and δλ are latitude error and longitude error, respectively. R_M and R_N are meridian curvature radius and unitary circle curvature radius.

(2) δ V x ˙ = ( 2 Ω cos ⁡ φ V y + V x V y R N sec 2 ⁡ φ ) δ φ + V y R N tan ⁡ φ δ V x + 2 Ω sin ⁡ φ V y + V x R N tan ⁡ φ δ V y + A y γ − g β + Δ A x . δ V y ˙ = − ( 2 Ω cos ⁡ φ V x + V x 2 R N sec 2 ⁡ φ ) δ φ − 2 ( Ω sin ⁡ φ + V x R N tan ⁡ φ ) δ V x + g α − A x γ + Δ A y .

The velocity error is shown in Eq. (2). δV_x and δV_y are east velocity error and north velocity error, respectively. Ω is the earth rotation rate, ΔA_x and ΔA_y are the accelerometer constant bias.

α, β and ϒ are the angles between the platform system p and the geographic n. The attitude angle error equations are as follows.

(3) α ˙ = − δ V y R M + ( Ω sin ⁡ φ + V x R N tan ⁡ φ ) β − ( Ω cos ⁡ φ + V x R N ) γ + ε x . β ˙ = − Ω sin ⁡ φ δ φ + δ V x R N − ( Ω sin ⁡ φ + V x R N tan ⁡ φ ) α − V y R M γ + ε y . γ ˙ = ( Ω cos ⁡ φ + V x R N sec 2 ⁡ φ ) δ φ + tan ⁡ φ R N δ V x + ( Ω cos ⁡ φ + V x R N ) α + V y R M β + ε z .

ε_x, ε_y and ε_z are gyro constant drifts.

The gyro errors and accelerometer errors after calibration are approximately random constants and Gaussian white noises. The gyro drift error is as follow.

(4) ε i = ε b i + w g i . i = x , y , z

w_gi(i = x, y, z) is Gaussian white noise with zero mean. The gyro random constant drift error is as follow.

(5) ε b i ˙ = 0. i = x , y , z

The accelerometer bias error is as follow.

(6) Δ A i = Δ A b i + w a i . i = x , y

w_ai(i = x, y) is Gaussian white noise with zero mean. The accelerometer random constant error is as follow.

(7) Δ A ˙ b i = 0. i = x , y

2.3 INS/gravity gradient aided navigation filter model

2.3.1 State equation

According to the INS error equations, taking the East-North-Up coordinate as navigation coordinate. The position errors, velocity errors, platform attitude errors, accelerometer bias errors and gyro drift errors are selected as the state vectors of the INS/gravity gradient aided navigation filter model.

(8) x ( t ) = [ δ φ δ λ δ V x δ V y α β γ Δ A x Δ A y ε x ε y ε z ] . T

The state equation of the INS/gravity gradient aided navigation is as follow.

(9) x ( t ) ˙ = A ( t ) x ( t ) + B ( t ) w ( t ) .

The system state transition matrix A can be derived from Eq. (1) ~(7).

(10) A = A 11 A 12 0 2 × 2 0 2 × 1 0 2 × 5 A 21 A 22 A 23 A 24 I 5 × 5 0 5 × 12 .

Where, I is unit matrix, 0_2×2 is zero matrix. A₁₁, A₁₂, A₂₁, A₂₂, A₂₃, A₂₄ are as shown below.

A 11 = 0 0 V x R N tan ⁡ φ sec ⁡ φ 0 .

A 12 = 0 1 R M sec ⁡ φ R N 0 .

A 21 = 2 Ω cos ⁡ φ V y + V x V y R N sec 2 ⁡ φ 0 − 2 Ω cos ⁡ φ V x − V x 2 R N sec 2 ⁡ φ 0 0 0 − Ω sin ⁡ φ 0 Ω cos ⁡ φ + V x R N sec 2 ⁡ φ 0 .

A 22 = V y tan ⁡ φ R N 2 Ω sin ⁡ φ + V x R N tan ⁡ φ − 2 Ω sin ⁡ φ − 2 V x R N tan ⁡ φ 0 0 − 1 R M 1 R N 0 tan ⁡ φ R N 0 .

A 23 = 0 − g g 0 0 Ω sin ⁡ φ + V x R N tan ⁡ φ − Ω sin ⁡ φ − V x R N tan ⁡ φ 0 Ω cos ⁡ φ + V x R N V y R M .

A 24 = A y − A x − Ω cos ⁡ φ − V x R N − V y R M 0 .

The system noise w(t) is as follows.

(11) w ( t ) = [ 0 0 w a x w a y w g x w g y w g z 0 0 0 0 0 ] . T

Where, w_ax, w_ay, w_gx, w_gy, w_gz are Gaussian white noise with zero mean, respectively. Corresponding to accelerometer bias errors and gyro random drift errors. System noise matrix is B = I_12×12.

Suppose the filter period is T, and the discrete state equations as follows.

(12) X k + 1 = A k X k + B k W k .

Where,

(13) A k = I + T A ( t ) .

(14) B k = T ( I + T 2 A ( t ) ) B ( t ) .

2.3.2 Observe equation

Taking the gravity gradient data as system observation, and the gravity gradient measurement data has a corresponding relationship with the underwater vehicle position, the observation equation of INS/gravity gradient aided navigation system is as follow.

(15) y j k = Γ j k ( φ , λ ) + v .

Where, y_jk is gravity gradient measurement data, which are 5 × 1 dimensions, and the subscript y, k = x, y, z represents the spatial direction; (φ, λ) represents the real position of the underwater vehicle; Γ_jk(φ, λ) represents the gravity gradient data at the real position; v is the obserbing Gaussian white noise.

Eq. (15) is difficult to express with a precise analytic function, which lead to the inability of the observation equation to be modeled. Since the particle filter estimates the probability distribution of the states by discrete randomly sampled particles and their weights, it is not necessary to obtain the specific form of the observation equation, and the processing can be processed by obtaining the correspondence between them. During the navigation of the underwater vehicle, the real position (φ, λ) is not available, and it can be replaced by the INS indicated position ( φ ^ , λ ^ ) and the position errors (δφ, δλ). The observation equation can be expressed as follow.

(16) y j k = Γ j k ( φ ^ + δ φ , λ ^ + δ λ ) + v .

3 Particle filter based on gravitation field algorithm

3.1 Particle filter

Particle filter is an approximate Bayesian filter method based on sequential Monte Carlo. The method approximates the current probability distribution by using a random sequence composed of finite particles and their weight, and continuously updates and recurs according to the particle filter algorithm.

The state equation and the observation equation of the nonlinear system are as follow.

(17) x k = f ( x k − 1 , w k ) . y k = h ( x k , v k ) .

Where, x_k is the state vector; y_k is the observation vector; w_k is the process noise; v_k is the observation noise.At the k time, particle filter obtains a new particle set by predictive sampling.

(18) P k = { ( x k i , w k i ) | i = 1 , … , N } .

Using the Eq. (19) to approximate the posterior probability density at k time.

(19) P ( x k | y 1 : k ) ≈ ∑ i = 1 N ω k i δ ( x k − x k i ) .

Where, N is the numbers of particles, δ(·) is the Dirichlet function, x k i and ω k i are the ith particle and its normalized weight,respectively. y_1:k is all observations before the k time. According to the principle of sequential importance sampling, the update importance weight equation is as follow.

(20) ω k i ∝ ω k − 1 i p ( y k | x k i ) ⋅ p ( x k i | x k − 1 i ) q ( x k i | x k − 1 i , y k ) .

State estimation equation.

(21) x k = ∑ i = 1 N ω k i x k i .

At the k = 0 time, particle filter initializes the sample set, and determines the prior probability of the particle state and initial value. At the next moment, particles update the state according to the system state transition equation, and calculate the weight of all particles according to the observations. Then, the output of posterior probability is obtained.

3.2 Gravitation field algorithm

The gravitation field algorithm(GFA) proposed in this paper is a new heuristic search algorithm which simulates the solar system nebula model [29, 30, 31]. The GFA mainly includes three steps, calculation the attractive force from the central dust, calculation the rotation repulsive force and position update. The virtual force model used in the GFA does not have fixed form. The GFA is described as follows.

3.2.1 The attractive force of central dust

In order to move the dust to the high-likelihood region, it is prescribed that all the dust is attracted by the central dust x_best, and the rest dust do not generate attractive force at the k time. The virtual mechanical model is as follow

(22) P = K a d i ( x b e s t − x i ) .

Where, P represents the virtual attractive force of x_best on x_i; K_a is the attractive force coefficient used to adjust the attractive force strength; d_i is the Euclidean distance between x_i and x_best.

3.2.2 The rotation repulsive force

While all the dust is attracted by the central dust, all the dust generates rotation repulsive force to the dust within the perceived range, so that the optimized objects are sparse and avoid excessive concentration. The virtual mechanical model is as follows [32].

(23) F i j = K r ( 1 d i j 2 − 1 d t h 2 ) ( x j − x i ) d i j .

Where, F_ij is the virtual rotation repulsive force of x_j on x_i, d_th is the perceived radius, K_r is the rotation repulsive force coefficient used to adjust the rotation repulsive force strength, and d_ij is the Euclidean distance between x_i and x_j.

3.2.3 Position update

The resultant force of the individual x_i is defined as the vector sum of all the virtual forces.

(24) F i = ∑ F i j + P . j = 1 , 2 , … , n , j ≠ i

Where, n is the amount of dust. x_i complete one iteration and update position by Eq. (25).

(25) x i ′ = x i + F i .

Where, x i ′ is the updated position. Its value is limited by the minimum displacement and maximum displacement, namely x i ′ ∈ [ L m i n , L m i n ] . The GFA terminates after ′the optimization condition or maximum iterations is satisfied.

3.3 Particle filter based on gravitation field algorithm

Traditional particle filter resampling method avoids particles impoverishment by duplicating large weight particles and deleting smaller weight particles. After iterations, it brings the problem of particle diversity dilution. To alleviate the above problems, the gravitation field algorithm is introduced into the resampling process of particle filter. The virtual attractive force makes the particles rapidly concentrate to the optimal particle. At the same time, the virtual rotation repulsive force makes the optimization particles avoid overlapping or crowding, guarantees the diversity of particles and the distribution of the posterior probability density. Therefore, the GFA has strong global optimization ability and faster convergence speed. After prediction, GF-PF treats candidate particle set as dust in the nebula model. Through the virtual force between the dust, a new suggested distribution is generated, and then the weight of the new particle set is updated and resampled. The computational steps of the GFA are as follows, and the flow chart is shown in Figure 2.

Figure 2

Flow chart of particle filter based on gravitation field algorithm.

Step 1. Initialization. Sampling x 0 i | i = 1 N from p(x₀), the weight ω 0 i is set to 1 N . Set the perception radius d_th, virtual force coefficient K_a and K_r,maximal iteration number EndMax.

Step 2. Prediction. Sample new particle set from x k i ∼ q ( x k i | x k − 1 i , y k ) and calculate the weights of particles.

Step 3. Optimize particle distribution. Calculate the Euclidean distance dist(s_i , s_j) between particles.

The particle with the largest weight is regarded as the center dust, and the virtual attractive force from center dust to the surrounding dust is calculated by Eq. (22).

Eq. (23) is used to calculate the virtual rotation repulsive force of dust on the other dust within the perceived range.

Eq. (24) is used to calculate the resultant force F_i of particles.

The position of particles is updated by Eq. (25).

Step 4. Get new particle set, and complete iterative. Recalculate new particle¡¯s weight and normalize.

Step 5. Resampling. If the number of valid particle N_eff is less than the set threshold N_th, resample the particle filter algorithm and return to { x ~ k t , ω ~ k t } | i = 1 N .

Step 6. State Estimation.

x ^ k i = ∑ i = 1 N ω ~ k i x ~ k i

4 Experiment

4.1 Track and gravity gradient maps used in the test

The INS output period is 1s, data recording is 7 hours, gyro constant drift errors are ε_x = ε_y = ε_z = 0.01^∘/h, accelerometer bias errors are ΔA_x = ΔA_y = 100μg, platform initial error angles are α = 0.1^∘, β = 0.1^∘ and ϒ = 0.5^∘, respectively. Due to the errors of gyros and accelerometers, the position errors of INS increases with time, as showed in Figure 3.

Figure 3

Position errors of inertial navigation system.

GPS and INS tracks are shown in Figure 4. The Pentagram is the starting position of GPS record. The underwater vehicle first sails southeast. After 2 hours, the underwater vehicle turns to the northwest.

Figure 4

GPS and INS tracks.

Gravity gradient maps are obtained by forward modeling of free air gravity anomaly data. The resolution is 30^′′×30^′′, including five independent tensors, such as Γ_xx, Γ_xy, Γ_xz, Γ_yy and Γ_yz. Data characteristics of gravity gradient maps are shown in Table 1.

Table 1

Data statistics of gravity gradient maps

	Min(E)	Max(E)	Mean(E)	Std(E)
Γ_xx	−41.6567	47.5624	−0.2495	4.4941
Γ_xy	−30.1590	33.1530	−0.0698	3.0714
Γ_xz	−69.2312	72.2177	0.2195	5.9581
Γ_yy	−54.4439	60.6621	−0.6259	4.7291
Γ_yz	−92.4407	66.4486	−0.7001	5.6174

During the simulation, the real-time gravity gradient data is simulated by superimposing Gaussian white noise on the gravity gradient data at the actual position of the underwater vehicle (provided by GPS). Data that is not on the grid is obtained by bilinear interpolation.

4.2 Application and analysis of GF-PF in INS/gravity gradient aided navigation

The GF-PF is applied to INS/gravity gradient aided navigation and compared with particle swarm optimization particle filter and artificial physics optimized particle filter [20] to verify the performance.

Simulation conditions: INS running alone for 2 hours, and then introduce INS/gravity gradient aided navigation. The initial position errors of GF-PF, PSO-PF and APO-PF is 2000m, the population of particles is 100, the maximum number of iterations is 20, and the filter period is 10s. The gyro constant drift errors and accelerometer bias errors are taken as the system noises and the observation noise is σ v 2 = 1 E . GF-PF parameters setting as: perceived radius is dth = 20m, attractive force coefficient is K_a = 10, rotation repulsive force coefficient is K_r = 1, maximum displacement is L_max = 30m, minimum displacement is L_min = L_max. The initial parameters of the PSO-PF are set as follows [33]: inertial weight is 0.99, acceleration constant is c₁ = c₂ = 0.1. The initial parameters of the APO-PF are set as follows [20]: attractive force coefficients are K_a1 = 10, K_a2 = 1,repulsive force coefficient is K_b = 0.9, perceived radius is r_s = 150m, threshold value is D_th = 10m.

10 independent simulation experiments of gravity gradient aided navigation are carried out using PSO-PF, APO-PF and GF-PF respectively. Figure 5 shows the tracks of the real track (GPS), INS track, PSO-PF matching track, GF-PF matching track and APO-PF matching track after INS running alone for 2 hours.

Figure 5

PSO-PF,APO-PF and GF-PF matching tracks.

The matching tracks of GF-PF, PSO-PF and APO-PF all track the real track well. PSO-PF and GF-PF have faster convergence speed, and APO-PF has the slowest convergence speed.

The root mean square error (RMSE) of latitude error and longitude error at time is as follow.

(26) RMSEt=(110∑m=110‖(φ^,λ^)tGFPF,(m)−(φ,λ)tGPS,(m)‖2)12.

Where, ( φ ^ , λ ^ ) t G F P F , ( m ) is the underwater vehicle longitude error and latitude error of the GF-PF at the t time obtained in the mth experiment, and ( φ , λ ) t G P S , ( m ) is the real latitude and longitude of the underwater visual at the t time.

After 10 independent simulations using GF-PF, PSO-PF and APO-PF respectively, the RMSE of the longitude and latitude errors is shown in Figure 6.

Figure 6

Position error curves of PSO-PF, APO-PF and GF-PF.

When the INS/gravity gradient aided navigation start working, the INS longitude error is about 414m, and the latitude error is about 1318m. At about 4th hour, the latitude error of the APO-PF has a large error. The latitude and longitude errors of PSO-PF are relatively large. Between the 3rd hour and the 7th hour, the RMSE of the PSO-PF algorithm is 214.23m, the RMSE of the APO-PF is 128.56m, and the RMSE of the GF-PF is 48.15m. The convergence speed of GF-PF and PSO-PF is almost same, which is better than APO-PF. Compared with the PSO-PF and APO-PF, the performance of GF-PF is more stable, and the positioning accuracy is higher.

4.3 Compare the effects of observation noise on the performance of each algorithm

Increase the observation noise to σ v 2 = 5 E , and other conditions and track remain same. The performance of each algorithm is compared. After 10 independent simulations using GF-PF, PSO-PF and APO-PF respectively, the RMSE of the longitude and latitude errors is shown in Figure 7.

Figure 7

Position error curves of PSO-PF, APO-PF and GF-PF(map resolution 30^′′ × 30^′′, gradient measure accuracy 5E).

At about 4th hour, the latitude error of the APO-PF has a large error. PSO-PF has the largest latitude error fluctuation. Between the 6th hour and the 7th hour, the longitude error and latitude error of the PSO-PF and APO-PF shows a large error. The latitude error of the GF-PF has a greatly fluctuates, but the longitude error maintains a high positioning accuracy. Between the 4th hour and the 7th hour, the latitude error of GF-PF is higher than APO-PF in part time. Between the 3rd hour and the 7th hour, the RMSE of the PSO-PF algorithm is 1303.01m, the RMSE of the APO-PF is 482.66m, and the RMSE of the GF-PF is 376.13m. Compared with the PSO-PF and APO-PF, the performance of GF-PF is more stable, and the positioning accuracy is higher. After increasing the observation noise to σ v 2 = 5 E , the convergence speed of GF-PF is slower than that of PSO-OF, but faster than APO-PF.

4.4 Compare the effects of gravity gradient maps resolution on the performance of each algorithm

Gravity gradient maps with resolution 1^′ × 1^′ are obtained by interval point extraction, and other conditions and track remain same. The performance of each algorithm is compared. After 10 independent simulations using GF-PF, PSO-PF and APO-PF respectively, the RMSE of the longitude and latitude errors is shown in Figure 8.

Figure 8

Position error curves of PSO-PF, APO-PF and GF-PF(map resolution 1^′ × 1^′, gradient measure accuracy 1E).

Between the 6th hour and the 7th hour, the latitude error of the PSO-PF and APO-PF shows a large error. The latitude error of the APO-PF maintains a stable effect, but the positioning error is higher than the GF-PF algorithm. Between the 4th hour and the 5th hour, the longitude error of the PSO-PF and GF-PF shows a large error. At about 6th hour, the longitude error of APO-PF has a large error. At about 7th hour, the longitude error of PSO-PF has a large error. Between the 3rd hour and the 7th hour, the RMSE of the PSO-PF algorithm is 462.27m, the RMSE of the APO-PF is 325.08m, and the RMSE of the GF-PF is 140.70m. Compared with the PSO-PF and APO-PF, the performance of GF-PF is more stable, the convergence speed is faster, and the positioning accuracy is higher.

5 Conclusion

Particle filter can avoid the linearization problem in INS / gravity gradient aided navigation. The intelligent optimization algorithm can effectively solve the particle degradation and sample depletion problems of the traditional particle filter algorithm. In this paper, the gravitation field algorithm is introduced into the resampling of the particle filter. The virtual forces are used to optimize the particle distribution, maintain the particle diversity, and form a uniform coverage capability for the posterior probability density. Applied the GF-PF to INS/gravity gradient aided navigation and compares it with PSO-PF and APO-PF. The results show that the positioning accuracy of GF-PF is higher than that of PSO-PF and APO-PF. The convergence speed of GF-PF and PSO-PF is almost same, which is better thanAPO-PF. Under different test conditions of observation noise and gravity gradient map resolutions, compared with PSO-PF and APO-PF, GF-PF still maintains the highest positioning accuracy and faster convergence speed. The performance of GF-PF is less affected by observation noise and gravity gradient map resolutions than that of PSO-PF and APO-PF.

Acknowledgement

This paper is supported by Natural Science Foundation of China (No. 6163308).

References

[1] Yao J.Q., The method of gravity gradient aided positioning based on STUPF, Navigation Positioning & Timing, 2018, 5, 60-63.Search in Google Scholar

[2] QinW.T., Guo J.F., Cui N.G. and Wang X.G., INS/GGI nav- igation in arctic area based on high degree cubature kalman filter, Tactical Missile Technology, 2018, 6, 85-94.Search in Google Scholar

[3] Tchier F., Inc M., Kilic B. and Akgul̈A., On soliton structures of generalized resonance equation with time dependent coefficients, Optik, 2017, 128, 218-223.10.1016/j.ijleo.2016.09.103Search in Google Scholar

[4] Boutarfa B., Akgul̈A. and Inc M., New approach for the Fornberg– Whitham type equations, Journal of Computational and Applied Mathematics, 2017, 312, 13-26.10.1016/j.cam.2015.09.016Search in Google Scholar

[5] Akgul̈A., New reproducing kernel functions, Mathematical Problems in Engineering, 2015, 1-11.10.1155/2015/181536Search in Google Scholar

[6] Akgul̈A. and Kilic¸man A., Solving delay differential equations by an accurate method with interpolation, Abstract and Applied Analysis, 2015, 1-8.10.1155/2015/676939Search in Google Scholar

[7] Han Y.R., Wang B., Deng Z.H. and Fu M.Y., A combined matching algorithm for underwater gravity aided navigation, IEEE/ASME Transactions on Mechatronics, 2018, 23, 233-241.10.1109/TMECH.2017.2774296Search in Google Scholar

[8] Wu L., Wang H.B., Chai H.,Zhang L., Hsu H. and Wang Y., Analysis and selection of global marine gravity/gravity gradi- ent aided navigation matching areas,The 9th China Satellite Navigation Conference, Harbin, China, 2018, 887-895.10.1007/978-981-13-0029-5_74Search in Google Scholar

[9] Inc M.,Akgul̈A. and KJlJc¸man A., Numerical solutions of the second-order one-dimensional telegraph equation based on reproducing kernel Hilbert space method, Abstract and Applied Analysis, 2013, 1-14.10.1155/2013/768963Search in Google Scholar

[10] Han Y.R., Wang B., Deng Z.H. and Fu M.Y., An improved TERCOM based algorithm for gravity aided navigation, IEEE Sensors Journal, 2016, 16, 2537-2544.10.1109/JSEN.2016.2518686Search in Google Scholar

[11] Wang K.D., Zhu T.Q., Qin Y.J., Jiang R., and Li Y., Matching error of the iterative closest contour point algorithm for terrain-aided navigation, Aerospace Science and Technology, 2018, 73, 210–222.10.1016/j.ast.2017.12.010Search in Google Scholar

[12] Akgul̈A., Hashemi M. S., Inc M. and RaheemS. A., Con- structing two powerful methods to solve the Thomas-Fermi equation, Nonlinear Dynamics, 2017, 87, 1435-1444.10.1007/s11071-016-3125-2Search in Google Scholar

[13] Wei E.D., Dong C.J., Yang Y.L. and Tang S.Q., A robust solution of integrated SITAN with TERCOM algorithm: weight-reducing iteration technique for underwater vehicle’s gravity-aided inertial navigation system, Navigation, 2017, 64, 111-122.10.1002/navi.176Search in Google Scholar

[14] Xiao J., Qi X.H. and Duan X.S., Research status of magnetic matching algorithm and its improvement strategies, Electronics Optics & Control, 2018, 25, 55-59, 73.Search in Google Scholar

[15] Zheng H., Wang Y., Wang H.B. and Wu L.,Simulation re- search of earth’s gravity and geomagnetism potential field aided underwater navigation, Geomatics & Information Science of Wuhan University, 2012, 37, 1198-1202.Search in Google Scholar

[16] Zhao L., Gao N., Huang B.Q., Wang Q.Y. and Zhou J.H., A novel terrain-aided navigation algorithm combined with the TERCOM algorithm and particle filter, IEEE J. Sensors, 2015, 15, 1124–1131.10.1109/JSEN.2014.2360916Search in Google Scholar

[17] Seitz J., Vaupel T. and Thielecke J., A particle filter for Wi-Fi azimuth and position tracking with pedestrian dead reckoning, 2013 Workshop on Sensor Data Fusion: Trends, Solutions, Applications(SDF), Bonn, Germany, 2013, 1-6.10.1109/SDF.2013.6698251Search in Google Scholar

[18] Akgul̈A., Inc M., Kilicman A. and Baleanu D., A new ap- proach for one-dimensional sine-Gordon equation, Advances in Difference Equations, 2016, 8, 1-20.10.1186/s13662-015-0734-xSearch in Google Scholar

[19] Hashemi M. S., Inc M., Kilic B. and Akgul̈A., On solitons and invariant solutions of the Magneto-electro-elastic circular rod, Waves in Random and Complex Media, 2016, 26(3), 259- 271.10.1080/17455030.2015.1124153Search in Google Scholar

[20] Liu F.M., Qian D. and Liu C.H., An artificeal physics optimized particle filter, Control and Decision, 2012, 27, 1145- 1149, 1156.Search in Google Scholar

[21] Zhang J.C., Kang F.J., Liang H.T. and Xu H., Research on chicken swarm optimization based particle filter, Journal of System Simulation, 2017, 29, 295-300, 308.Search in Google Scholar

[22] Gao M.L., Li L.L., Sun X.M., Yin L.J. and Li H.T., Firefly algorithm (FA) based particle filter method for visual tracking, Optik, 2015, 126, 1705-1711.10.1016/j.ijleo.2015.05.028Search in Google Scholar

[23] Chen Z.M., Tian M.C., Wu P.L., Bo Y.M., Gu F.F., and Yue C., Intelligent particle filter based on bat algorithm, Acta Phys. Sin., 2017, 66, 41-50.10.7498/aps.66.050502Search in Google Scholar

[24] Liu F.M., Wei X., Qian D. and Li F.M., The application of optimization particle filter in gravity aided positioning, Journal of Harbin Institute of Technology, 2012, 44, 145-148.Search in Google Scholar

[25] Wang B., Yu L., Deng Z.H. and Fu M.Y., A particle filter- based matching algorithm with gravity sample vector for underwater gravity aided navigation, IEEE/ASME TRANSAC- TONS ON MECHATRONICS, 2016, 21, 1399-1406.10.1109/TMECH.2016.2519925Search in Google Scholar

[26] Liu F.M. and Li F.M., Application of self-adaptive artificial physics optimized particle filter in INS/gravity gradient aided navigation, Proceedings of 2018 IEEE International Conference on Mechatronics and Automation, Changchun, China, 2018, 1070-1075.10.1109/ICMA.2018.8484416Search in Google Scholar

[27] Gao G.D., Lin M. and XU L., New PSO particle filter method based on likelihood-adjustment, Journal of computer applications, 2017, 37, 980-985.Search in Google Scholar

[28] Wang Z.Y. and Sun F., Rao-Blackwellised Particle Filter and It’s Application in Navigation, Journal of Projectiles, Rockets, Missiles and Guidance, 2013, 33, 153-159.Search in Google Scholar

[29] Liu F., Li F., Lin N. and Jing X., Gravity aided positioning based on real-time ICCP with optimized matching sequence length, IEEE Access, 2019, 7, 97440-97456.10.1109/ACCESS.2019.2929778Search in Google Scholar

[30] Akgul̈A., Khan Y., Akgul E. K., Baleanu D. and Qurashi M. M. A., Solutions of nonlinear systems by reproducing kernel method, The Journal of Nonlinear Sciences and Applications, 2017, 10, 4408-4417.10.22436/jnsa.010.08.33Search in Google Scholar

[31] Inc M., Akgul̈A. and Kilic¸man A., A novel method for solving KdV equation based on reproducing kernel Hilbert space method, Abstract and Applied Analysis, 2013, 1-12.10.1155/2013/578942Search in Google Scholar

[32] Ding C.Y. and Peng J., A hooping sensor deployment scheme based on virtual force, Proceedings of 2015 IEEE Conference on Robotics and Biomimetics, Zhuhai ,China, 2015, 988-993.10.1109/ROBIO.2015.7418900Search in Google Scholar

[33] Bonyadi M. and Michalewicz Z., Impacts of coefficients on movement patterns in the particle swarm optimization algorithm, IEEE Transactions on Evolutionary Computation, 2017, 21, 378-390.10.1109/TEVC.2016.2605668Search in Google Scholar

Received: 2019-05-12

Accepted: 2019-09-02

Published Online: 2019-12-30

This work is licensed under the Creative Commons Attribution 4.0 International License.

Articles in the same Issue

https://doi.org/10.1515/phys-2019-0073

Keywords for this article

gravitation field; solar nebular disk mode; particle filter; gravity gradient; virtual forces

Creative Commons

BY 4.0