Solving Gravity Anomaly Matching Problem Under Large Initial Errors in Gravity Aided Navigation by Using an Affine Transformation Based Artificial Bee Colony Algorithm

Gravity aided inertial navigation system (GAINS), which uses earth gravitational anomaly field for navigation, holds strong potential as an underwater navigation system. The gravity matching algorithm is one of the key factors in GAINS. Existing matching algorithms cannot guarantee the matching accuracy in the matching algorithms based gravity aided navigation when the initial errors are large. Evolutionary algorithms, which are mostly have the ability of global optimality and fast convergence, can be used to solve the gravity matching problem under large initial errors. However, simply applying evolutionary algorithms to GAINS may lead to false matching. Therefore, in order to deal with the underwater gravity matching problem, it is necessary to improve the traditional evolutionary algorithms. In this paper, an affine transformation based artificial bee colony (ABC) algorithm, which can greatly improve the positioning precision under large initial errors condition, is developed. The proposed algorithm introduces affine transformation to both initialization process and evolutionary process of ABC algorithm. The single-point matching strategy is replaced by the strategy of matching a sequence of several consecutive position vectors. In addition, several constraints are introduced to the process of evolution by using the output characteristics of the inertial navigation system (INS). Simulations based on the actual gravity anomaly base map have been performed for the validation of the proposed algorithm.


INTRODUCTION
It is well known that inertial navigation systems (INSs) typically used on underwater vehicles tend to develop accumulated errors. Above water, the INS data can be corrected by the use of global navigation satellite system (GNSS) (Bishop, 2002). However, due to the rapid attenuation of higher frequency signals and the unstructured nature of the undersea environment, GNSS signals can only propagate within short distance under water (Paull et al., 2013). As a passive navigation system, the gravity aided inertial navigation system (GAINS) holds strong potential as an auxiliary navigation system (Canciani and Raquet, 2017). GAINS obtains a gravity anomaly measurements by using gravimeters installed on a vehicle. These measurements are matched with a priori digital map of the gravity anomaly, to estimate the vehicle position. GAINSs are completely passive and difficult to interfere with. These advantages are totally meet the requirement of underwater vehicles (Rice et al., 2004).
A GAINS consists of a priori gravity anomaly database (gravity anomaly map), a measurement unit (gravimeter), and a navigation algorithm. There are two types of GAINSs: filtering algorithms based GAINSs Claus and Bachmayer, 2015;Copp and Subbarao, 2015;Allotta et al., 2016) and matching algorithms based GAINSs Wu et al., 2015;Zhu et al., 2015;Han et al., 2016;Song et al., 2016). Filtering algorithms, which take use of simplified dynamic mathematical model of the vehicle, have a good performance in real-time. In filtering algorithms based GAINSs, commonly used filters are the extended Kalman filter (EKF), the particle filter (PF) and the Rao-Blackwellized particle filter (RBPF). EKF is more effective for low nonlinear estimation problems. PF can effectively handle highly non-linear or non-Gaussian estimation problems. RBPF is a hybrid filter combining EKF and PF (Simanek et al., 2015;Kim T. et al., 2018;Kim Y. et al., 2018). However, the application of filtering algorithms is limited since the precise model of gravity anomaly is difficult to establish (Han et al., 2016). The matching algorithm is a key factor in matching algorithms based GAINSs (Hegrenaes and Hallingstad, 2011;Wu et al., 2017). Terrain contour matching (TERCOM) (Affleck and Jircitano, 1990) algorithm and iterative closest contour point (ICCP) (Kamgarparsi and Kamgarparsi, 1999) algorithm are two conventional matching algorithms in GAINSs. TERCOM algorithm is realized via group correlation analysis. Most of the improvements to the TERCOM algorithm Han et al., 2016) are proposed to solve bad real-time performance and heavy computational complexity problem. However, TERCOM algorithm is sensitive to angular error of the INS-indicated segment, which is difficult to improve. ICCP algorithm, which uses rigid transformation to matching the multilateral arc, has a high matching accuracy. Over the past decade, a number of improved ICCP algorithm have been suggested. Among them, a part of researches were developed for improving the real-time performance (Tong et al., 2011;Wang et al., 2013). Other studies adopted affine transformation to deal with the scale error (Xu et al., 2014;Song et al., 2016). However, the mismatching of ICCP algorithm is easily included when the initial position errors of INS are large (Han et al., 2016). Both TERCOM-based algorithms and ICCP-based algorithms cannot guarantee the matching accuracy when initial errors are large. TERCOM-based algorithms are sensitive to angular errors of the INS-indicated segments and ICCP-based algorithms are sensitive to position errors. Therefore, a matching algorithm that maintain high matching precision under large initial errors is needed. Evolutionary algorithms (Quan and Fang, 2010;Gao et al., 2014;Hidalgo-Paniagua et al., 2016;Teymourian et al., 2016;Li et al., 2017) are a good choice because most of them have the ability of both global optimality and fast convergence.
The evolutionary algorithms have emerged as a powerful tool for finding optimum solutions of complex optimization problems. In the past few decades, a number of evolutionary algorithms have been used extensively to obtain optimal designs and overcome the computational drawbacks of traditional mathematical optimization methods (Abrao, 2013;Yildiz, 2013). Jaesung and Kim improved the genetic algorithm so solve robot path planning problem (Lee and Kim, 2016). An improved intelligent water drops algorithm and an advanced cuckoo search algorithm are proposed to solve the capacitated vehicle routing problem (Teymourian et al., 2016). Improved bee colony algorithm is applied to the color image segmentation, assigning the optimal coordinates of seeds and determining similarity differences (Sag and Çunkaş, 2015).
Gravity matching problem can be regarded as an optimization problem (Wu et al., 2017). Therefore, evolutionary algorithms can be directly applied to the gravity matching problem. Gao et al. proposed an improved artificial bee colony (ABC) algorithm and obtained a good performance on gravity matching under small initial errors . However, there exists a lot of similarity feature points in matching area when initial errors are large. Although the improved ABC algorithm can be implemented on navigation systems under large initial errors, the mismatching of position is easily included. In this paper, we consider a challenging gravity matching problem of achieving high precision under large initial errors. To solve this problem, we propose an affine transformation based ABC algorithm. Firstly, In order to avoid mismatching caused by similarity feature points, we apply sequence matching strategy to the proposed algorithm. Secondly, Scaling transformation, rotation transformation, and translation transformation are introduced to both initialization process and evolutionary process to increase the convergence speed. In addition, we constrain the affine transformation by utilizing the output characteristics of INS.
The rest of the paper is organized as follows. The second part introduces the ABC algorithm. INS error propagation model and the constraints of affine transformation are provided in the third part. The fourth part of this paper presents the procedure of affine transformation based artificial bee colony algorithm. A comprehensive discussion on the experimental settings and simulation results are provided in the fifth part. Conclusions are given in the last part.

ARTIFICIAL BEE COLONY ALGORITHM
The ABC algorithm (Karaboga and Basturk, 2007) is one of the most recently introduced swarm-based search methods (Akay and Karaboga, 2012;Akbari et al., 2012;Sag and Çunkaş, 2015;Li et al., 2016). It contains three groups of bees: employed bees, onlookers, and scouts. In ABC algorithm, the position of a food source represents a possible solution of the optimization problem and the nectar amount of a food source corresponds to the quality (fitness) of the associated solution. The more a solution has high fitness, the more possibility of being selected of this solution by onlooker bees. The number of the employed bees or the onlooker bees is equal to the number of solutions in the population. The initial swarm composed of employed bees are generated by (1): Where x j i indicates the jth parameter of ith solution in the population, i = {1, 2, · · · , SN} and SN is the size of population, j = {1, 2, · · · , D} and D is the number of parameters in a solution. x j min and x j max are, respectively, the lower bound and the upper bound of jth parameter. rand(0, 1) generates a real number between 0 and 1.
After initialization, fitness values of solutions are calculated by (2): Where f i is the objective function value of ith solution in the population. fit i is the fitness value of ith solution in the population. The employed bee evaluates the quality of food sources and determines a new one according to the fitness value. If the nectar amount of new source is better than the old source, employed bee will update its memory with new source. The employed bees update their sources according to the following equation: Where x j neighbor is the neighbor solution selected randomly, neighbor ∈ {1, 2, · · · , i − 1, i + 1, · · · , SN}. v j i represents new solution.
After new food sources have been explored, onlookers select an employed bee for guidance. For this purpose, roulette wheel is used to calculate the probabilities: After the onlooker bees select a food source as a guide, the candidate food source is calculated by (3). Afterwards, the greedy selection process is applied for the onlooker bees. The best solution achieved so far is memorized. In addition, a food source will be abandoned when limit, a control parameter, is exceeded for the source. So it is replaced with randomly produced solution by (1). For the gravity matching problem, denote x as the position of vehicle and f as the gravity objective function. Then ABC algorithm can be directly used to find the optimal position. However, simply applying ABC algorithm to the gravity matching problem may lead to false matching . There exists a lot of similarity gravity feature points in gravity database, which means single-point matching process may easily convergence at the wrong position.

SEQUENCE MATCHING STRATEGY
To solve the mismatching problem, the single-point matching strategy in basic ABC is replaced by strategy of matching a sequence of several consecutive position vectors. Considering the short-term high-precision characteristics of the INS (Wu et al., 2015), affine transformation could be used to describe the relationship between the sequence to be matched and the INSindicated sequence. Specifically, these transformations include scaling transformation, rotation transformation, and translation transformation. The range of affine transformations can be estimated by an INS error propagation model.

INS Error Propagation Model
Affine transformation is constrained by the output characteristics of the INS. Therefore, an INS error propagation model (Yan et al., 2008) is employed in this paper. In Yan et al. (2008), ireference frame is the fixed inertial frame; e-reference frame is the Terrestrial reference frame (TRF); n-reference frame has its origin on the surface and its axes pointing East, North, and Up (ENU reference frame); b-reference frame is centered in the center of gravity of the vehicle, with the y-axis pointing in the direction of the forward motion of the vehicle, the z-axis pointing up and the x-axis completing a right-handed reference frame. However, GAINSs are commonly used in underwater environments. In which case n-reference frame adopts North, East and Down (NED) reference frame; b-reference frame is centered in the center of gravity of the vehicle, with the x-axis pointing in the direction of the forward motion of the vehicle, the z-axis pointing down and the y-axis completing a righthanded reference frame (Allotta et al., 2016). Hence the INS error propagation model in Yan et al. (2008) needs to be modified.
In actual case, there exist rotation errors between n-reference frame (ideal mathematics platform) and n'-reference frame (actual mathematics platform). n'-reference frame can be gained after rotating 3 times of n-reference frame. Let α z , α y , α x be the three rotational angles and α= α x α y α z , the rotation matrixes can be expressed as: Then, the coordinate transformation matrix from n-reference frame to n'-reference frame is derived: Denoting with ω n ′ nn ′ the relative angular velocity of n ′reference frame in n-reference frame, the following equation has been derived: Frontiers in Neurorobotics | www.frontiersin.org Hence, the differential equation of Euler platform error angles can be expressed as follows: Here C ω and C -1 ω are calculated by the following expressions: Denoting with ω n ie the rotational angular velocity of the earth. L and h are, respectively, latitude, and depth. R M is the local radius of curvature in meridian and R N is the local radius of curvature in prime vertical.L = L + δL,ĥ = h + δh. In addition, δL and δh are slight errors. The following equations have been derived: ω n in and δω n in can be expressed as follows: δω n in = δω n ie + δω n en (15) On the basis of these formulas, the INS attitude error and velocity error have been derived (Yan et al., 2008): Where δω b ib is the measurement error of gyroscope, δω n in is the calculation error of ω n in . δf b sf is the measurement error of accelerometer. In addition, δω b ib are mainly consist of a constant bias ε b and zero mean Gaussian white noise w b g . δf b sf are mainly consist of a constant bias ∇ b and zero mean Gaussian white noise w b a . Besides, δg n could be ignored. Hence, the INS error propagation model used in underwater environment is defined by the following equations:

Range of Scaling Transformation
Through the INS error propagation model (18), the range of rotation transformation and translation transformation can be estimated. However, the range of scaling transformation cannot be directly calculated. To better describe the scaling transformation, the INS-indicated distance and the actual distance in a short period are expressed by the following equations: Then, the relationship between d INS k-1,k and d real k−1,k is derived: Where v INS k is described by an equation in the form: Here δv k is the INS accumulated velocity error of kth sampling point. Accordingly, (21) is developed into the following form: Frontiers in Neurorobotics | www.frontiersin.org Denoting with N the number of sampling points on matching sequence, the constraint of scale transform has been derived:

AFFINE TRANSFORMATION BASED ABC ALGORITHM
In order to achieve high positioning accuracy under large initial errors, this paper proposes an affine transformation based ABC algorithm. The proposed algorithm is specifically presented to deal with the gravity matching problem for underwater navigation system. The affine transformation based ABC algorithm introduces affine transformation to both initialization process and evolutionary process of ABC algorithm. In addition, the affine transformation satisfies the constraint conditions provided by the output characteristics of the INS.

Objective Function and Initialization
The proposed algorithm use mean absolute difference (MAD) function as the objective function: Where x indicates a sequence of several consecutive position vectors. g obs k is the measured gravity anomaly value of kth sampling point and g x k is the corresponding gravity anomaly value on gravity anomaly base map of kth sampling point. f (x) is the objective function.
fit (x), the fitness value of x, is calculated by (26): Initialization process is the first step of the proposed algorithm. In ABC algorithm, the initial swarm composed of employed bees are given by (1). But the lower bound and the upper bound of the x in sequence matching are difficult to acquire. Thus, the initialization method should be redefined. The proposed algorithm applies scaling transformation, rotation transformation, and translation transformation on the INS-indicated sequence to implement initialization. Translation transformation is implemented by choosing a random first element of initial swarm: Where x L i,1 and x λ i,1 indicate the latitude and longitude of the first sampling point on sequence i, i = {1, 2, · · · , SN} and SN is the size of population. x L max and x L min are, respectively, the upper bound and the lower bound of latitude in search scope. x λ max and x λ min are, respectively, the upper bound and the lower bound of longitude in search scope.
Let S ini i be the scale factor used for initialization sequence i. The value of S ini i is calculated by (29): The expression of S is obtained from (24). The rotation angle used for initialization sequence i can be calculated by the following equation: Where β ini min and β ini max are the minimum angle error and the maximum angle error of a segment in INS-indicated trajectory. Thus, the rotation matrix used for initialization sequence i have been derived: . , x ini i,N be the ith initialization sequence. After above mentioned transformations have been performed, the initial swarm is obtained: Additional translation vector need to be applied on generated trajectories if they are out of range. Let init L min and init L max be the maximum and minimum latitude of initial trajectory. init λ min and init λ max are the maximum and minimum longitude of initial trajectory. T = T L , T λ , the translation vector, can be expressed as:

Employed Bee Phase
With the initialization complete, an employed bee updates its food source in the neighborhood. Suppose X ini i is the sequence to be updated, X ini j = x ini j,1 , x ini j,2 , . . . , x ini j,N is selected randomly in the neighborhood of X ini i , j ∈ {1, 2, · · · , i − 1, i + 1, · · · , SN} and SN is the size of population. The evolution equation is derived through the relation between X ini i and X ini j . Denoting with Q the covariance matrix: Frontiers in Neurorobotics | www.frontiersin.org Wherex ini i andx ini j are, respectively, the average value of all sampling points in X ini i and X ini j . The eigenvalues of Q and the rotation angle from X ini i to X ini j are calculated through quaternion algorithm (Berthold, 1987): Where λ µ (µ= {1, 2, 3, 4}) are four eigenvalues of Q. λ m is the maximum eigenvalue. τ is the rotation angle from X ini i to X ini j . Then, the rotation angle used for X ini i during employed bee phase is calculated by the following equations: Notice that the angle between X ini i and X INS after rotation should be in the range (β ini min , β ini max ). Denoting with η be the rotation angle from X ini i to X INS , β em i should meet the following in equation: Then, the following expression is obtained by (40) and (41): Thus, the rotation matrix used for X ini i during employed bee phase is calculated by (43): Let L ini i and L ini j be the length of X ini i and X ini j . L ins is the length of INS-indicated trajectory. The scale factor used for X ini i during employed bee phase is calculated by the following equations: Notice that the scale factor between X ini i and X INS after scaling transformation should meet the range on the right side of (29): Denoting with S em min and S em max the lower bound and the upper bound of S em i . According to (44) and (45), S em i is updated by (46): i ′ after rotation transformation and scaling transformation have been performed: Letx ini i ′ be the average value of all sampling points in X ini i ′ and the translation vector is calculated by (49): Then, the employed bees update their sources according to the following equation: If the fitness value of X em i is larger than the fitness value of X ini i , X ini i will be replaced by X em i . At the end of the employed bee phase, all of current sequences are denoted by X em .

Onlooker Bee Phase and Scout Bee Phase
The probability of each sequence to be selected by the onlooker bee is calculated by (4). Suppose X em i is the sequence selected by onlooker bee, the rotation matrix R on i , the scale factor S on i , and the translation vector T on i used on X em i during onlooker bee phase can be calculated by (43), (46),and (49). Thus, the onlooker bees update their sources according to the following equation: If the fitness value of X on i is larger than the fitness value ofX em i , X em i will be replaced by X on i . The best solution achieved so far is memorized. In addition, a sequence will be abandoned when limit, a control parameter, is exceeded for the source during scout bee phase. So it is replaced with randomly produced solution by (34). The pseudo of the proposal is given below.
Procedure of the proposal Begin 1. confirm search scope 2. initialize a matching sequence X ini i at generation t = 0 with trial(i) = 0 using (34) for i = 1,2,. . . ,SN 3. evaluate f X ini i for i = 1,2,. . . ,SN and set the best solution as X best 4. while termination condition is not reached do //employed bee phase 2) all of current sequences are denoted by X em //onlooker bee phase 3) set k = 1, while k<SN+1 do Select a sequence X em i based on its probability of selection calculated using (4) and produce a new solution X on i using (51) end while 4) all of current sequences are denoted by X on //scout bee phase 5) denote the best solution in X on as X best 8) all of current sequences are denoted by X ini end while 5. output X best End  Figure 1 (Supplementary Data Sheet 1). As seen from Figure 1, the position error of the generated trajectory exhibits a Scuhler oscillation of 84.4 min.
In order to evaluate the algorithm presented in this work, a gravity anomaly base map is required. In the simulations tests, goco05c model (Fecher et al., 2017) is used to calculate the gravity anomaly in the area from (147.6 • E, 19.5 • N) to (156.6 • E, 24 • N). After interpolated, the grid step is converted to 0.3 ′ . The 3-D map of the gravity anomaly data is shown in Figure 2  (Supplementary Data Sheet 1). The gravity anomaly relevant parameters are shown in Table 2.    Figure 3 shows the angle error of a segment in INS-indicated trajectory. It is obtained that the angle error is vary in a small range and the maximum segment angle error is 15 • . Thus, β ini min and β ini m ax are set as −15 • and 15 • , respectively. The proposed algorithm uses the 3σ principle to confirm the search scope (Han et al., 2016). Table 3 shows the configuration of the proposed algorithm. In this paper, optimal parameter values of affine transformation based ABC are obtained based on experience.

Maximum iterations 500
Limit 20 Population size 10 Number of sampling points per sequence 12 Sampling interval 5 min The variance of gravity anomaly measurement noise 1 mGal

Comparison Between Basic ABC and Affine Transformation Based ABC
Based on the parameters above, the first set of 50 Mont Carlo simulation tests were done on the trajectory after 3 h of sailing. Simulation results of introducing sequence  matching strategy into ABC algorithm has better positioning precision and matching probability than the basic ABC, as shown in Figures 4, 5. Statistical results of simulation tests are given in Table 4. In Table 4, a matching result is considered to be a successful matching if the position error is within 2 ′ .
Comparison Between ICCP, Improved-ABC and Affine Transformation Based ABC Due to ICCP is a widely used gravity matching algorithm and improved-ABC algorithm ) is presented to tackle the similar problem. We use these algorithms as references, comparing the results of FIGURE 7 | The average error of longitude and latitude. To test algorithms under large initial position errors, the second set of 50 Mont Carlo simulation tests were done on the trajectory after 7 h of sailing using the above three algorithms. Trajectories of matching results are shown in Figure 6, each trajectory is a typical one in 50 Mont Carlo simulations. Figure 7 indicates the average error of longitude and latitude of each sampling point in 50 Mont Carlo simulations. In order to better evaluate the proposed algorithm, the statistical results of all the three algorithms are given in Table 5.
Simulation results in Figures 6, 7, and Table 5 show that the matching accuracy of the proposed algorithm is significantly superior to the other two algorithms under large initial position errors.
To better observe the effect of initial errors on these three algorithms, matching results of the whole trajectory within 16 h are shown in Figure 8 and Table 6. The results are divided into three time periods for statistic. In Table 6, a matching result is considered to be a successful matching if the position error is within 5 ′ . It can be concluded that the convergence speed of ICCP algorithm decreases a lot with the increase of initial position errors. While the convergence speed of improved-ABC algorithm is not affected by initial position errors. The proposed algorithm converge slower than improved-ABC algorithm and much faster than ICCP algorithm under large initial errors. In Figure 8, at the beginning, all of the three algorithms can hold high positioning accuracy. But with the initial errors and the search scope increase, the accuracy of ICCP algorithm and improved-ABC algorithm decrease significantly, whereas the proposed algorithm can guarantee much higher matching accuracy.

CONCLUSION
Gravity aided inertial navigation system can solve the problem of INS error accumulation and ensure the concealment of INS. However, there are still many problems in the field of gravity aided navigation system. In this paper, we focus on the problem of how to ensure the matching accuracy under the large initial errors condition. In order to avoid mismatching under large initial errors, an affine transformation based ABC algorithm is adopted.
The simulation results show that the proposed algorithm can achieve high matching precision under the condition of large initial errors. In addition to ABC algorithm, other evolutionary algorithms can also be used to solve the gravity matching problem. How to introduce the sequence matching strategy in other evolutionary algorithms? How are the effects of these algorithms after introducing the sequence matching strategy? These need to be further studied in the future. What's more, the matching accuracy of gravity matching algorithm is highly correlated with the precision and resolution of the gravity anomaly base map. This paper focuses on the matching algorithm for large initial errors. How to perform a gravity matching algorithm in the area with low uniqueness should be discussed in the next step.

AUTHOR CONTRIBUTIONS
TD and LM designed the study. TD, LM, and HS performed the simulations. All of the authors analyzed the data. All authors contributed to the writing and editing of the manuscript.

FUNDING
This work was supported by the National Natural Science Foundation of China (grant number 61473039).