Solving Gravity Anomaly Matching Problem Under Large Initial Errors in Gravity Aided Navigation by Using an Affine Transformation Based Artificial Bee Colony 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 (Li et al., 2013; Claus and Bachmayer, 2015; Copp and Subbarao, 2015; Allotta et al., 2016) and matching algorithms based GAINSs (Zhao et al., 2014; 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 (Zhao et al., 2014; 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 (Gao et al., 2014). 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):

$\begin{array}{ll}{\text{x}}_{{}_i}^j={\text{x}}_{min}^j+rand(0,1)({\text{x}}_{max}^j-{\text{x}}_{min}^j)& (1)\end{array}$

Where ${\text{x}}_{{}_i}^j$ 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. ${\text{x}}_{min}^j$ and ${\text{x}}_{max}^j$ 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):

$\begin{array}{ll}fi{t}_i=\{\begin{array}{cc}1/(1+f_i)& f_i\ge 0\\ 1+\left|f_i\right|& f_i<0\end{array}& (2)\end{array}$

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:

$\begin{array}{ll}{\text{v}}_{{}_i}^j={\text{x}}_{{}_i}^j+rand(-1,1)({\text{x}}_{{}_i}^j-{\text{x}}_{neighbor}^j)& (3)\end{array}$

Where ${\text{x}}_{neighbor}^j$ is the neighbor solution selected randomly, neighbor ∈ {1, 2, ⋯ , i − 1, i + 1, ⋯ , SN}. ${\text{v}}_{{}_i}^j$ 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:

$\begin{array}{ll}{p}_i=\frac{fi{t}_i}{{\displaystyle \sum _{q=1}^{SN}}fi{t}_q}& (4)\end{array}$

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 (Gao et al., 2014). 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 INS-indicated 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), i-reference 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 right-handed 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:

$\begin{array}{ll}\begin{array}{cc}{C}_{{\alpha }_z}& =\left[\begin{array}{ccc}cos{\alpha }_z& -sin{\alpha }_z& 0\\ sin{\alpha }_z& cos{\alpha }_z& 0\\ 0& 0& 1\end{array}\right]\\ C_{{\alpha }_y}& =\left[\begin{array}{ccc}cos{\alpha }_y& 0& -sin{\alpha }_y\\ 0& 1& 0\\ sin{\alpha }_y& 0& cos{\alpha }_y\end{array}\right]\\ C_{{\alpha }_x}& =\left[\begin{array}{ccc}1& 0& 0\\ 0& cos{\alpha }_x& sin{\alpha }_x\\ 0& -sin{\alpha }_x& cos{\alpha }_x\end{array}\right]\end{array}& (5)\end{array}$

Then, the coordinate transformation matrix from n-reference frame to n’-reference frame is derived:

$\begin{array}{ll}{\text{C}}_{\text{n}}^{{\text{n}}^{\prime }}={\text{C}}_{{\alpha }_x}{\text{C}}_{{\alpha }_y}{\text{C}}_{{\alpha }_z}& (6)\end{array}$

Denoting with ${\omega }_{{\mathrm{\text{nn}}}^{\prime }}^{{\mathrm{\text{n}}}^{\prime }}$ the relative angular velocity of n′-reference frame in n-reference frame, the following equation has been derived:

$\begin{array}{ll}{\omega }_{{\text{nn}}^{\prime }}^{{\text{n}}^{\prime }}={\text{C}}_{{\alpha }_x}{\text{C}}_{{\alpha }_y}\left[\begin{array}{c}0\\ 0\\ {\dot{\alpha }}_z\end{array}\right]+{\text{C}}_{{\alpha }_x}\left[\begin{array}{c}0\\ {\dot{\alpha }}_y\\ 0\end{array}\right]+\left[\begin{array}{c}{\dot{\alpha }}_x\\ 0\\ 0\end{array}\right]={\text{C}}_{\omega }\left[\begin{array}{c}{\dot{\alpha }}_x\\ {\dot{\alpha }}_y\\ {\dot{\alpha }}_z\end{array}\right]& (7)\end{array}$

Hence, the differential equation of Euler platform error angles can be expressed as follows:

$\begin{array}{ll}\dot{\alpha }={\text{C}}_{\omega }^{-1}{\omega }_{{\text{nn}}^{\text{′}}}^{{\text{n}}^{\text{′}}}& (8)\end{array}$

Here C_ω and ${\text{C}}_{\omega }^{\mathrm{\text{-}}1}$ are calculated by the following expressions:

$\begin{array}{ll}\begin{array}{c}{\text{C}}_{\omega }=\left[\begin{array}{ccc}1& 0& -sin{\alpha }_y\\ 0& cos{\alpha }_x& sin{\alpha }_{x}cos{\alpha }_y\\ 0& -sin{\alpha }_x& cos{\alpha }_{x}cos{\alpha }_y\end{array}\right]\\ {\text{C}}_{\omega }^{-1}=\frac{1}{cos{\alpha }_y}\left[\begin{array}{ccc}cos{\alpha }_y& sin{\alpha }_{x}sin{\alpha }_y& cos{\alpha }_{x}sin{\alpha }_y\\ 0& cos{\alpha }_{x}cos{\alpha }_y& -sin{\alpha }_{x}cos{\alpha }_y\\ 0& sin{\alpha }_x& cos{\alpha }_x\end{array}\right]\end{array}& (9)\end{array}$

Denoting with ${\text{ω}}_{\mathrm{\text{ie}}}^{\mathrm{\text{n}}}$ 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. $\widehat{L}=L+\delta L$, ĥ = h + δh. In addition, δL and δh are slight errors. The following equations have been derived:

$\begin{array}{llll}{\omega }_{\text{ie}}^{\text{n}}& =& {\left[\begin{array}{ccc}{\omega }_{\text{ie}}cosL& 0& -{\omega }_{\text{ie}}sinL\end{array}\right]}^{\text{T}}& (10)\end{array}$

$\begin{array}{llll}{\omega }_{\text{en}}^{\text{n}}& =& {\left[\begin{array}{ccc}\frac{v_{\text{E}}^{\text{n}}}{R_{\text{N}}-h}& -\frac{v_{\text{N}}^{\text{n}}}{R_{\text{M}}-h}& -\frac{v_{\text{E}}^{\text{n}}}{R_{\text{N}}-h}tanL\end{array}\right]}^{\text{T}}& (11)\end{array}$

$\begin{array}{llll}\delta {\text{ω}}_{\text{ie}}^{\text{n}}& =& \left[\begin{array}{c}-{\omega }_{\text{ie}}sin\widehat{L}\delta L\\ 0\\ -{\omega }_{\text{ie}}cos\widehat{L}\delta L\end{array}\right]& (12)\end{array}$

$\begin{array}{llll}\delta {\text{ω}}_{\text{en}}^{\text{n}}& =& \left[\begin{array}{c}\delta v_{\text{E}}^{\text{n}}/\left({\widehat{R}}_{\text{N}}-ĥ\right)\\ -\delta v_{\text{N}}^{\text{n}}/\left({\widehat{R}}_{\text{M}}-ĥ\right)\\ -\left(tan\widehat{L}\delta v_{\text{E}}^{\text{n}}+{\widehat{v}}_{\text{E}}^{\text{n}}{sec}^2\widehat{L}\delta L\right)/\left({\widehat{R}}_{\text{N}}-ĥ\right)\end{array}\right]& (13)\end{array}$

${\text{ω}}_{\mathrm{\text{in}}}^{\mathrm{\text{n}}}$ and $\delta {\text{ω}}_{\mathrm{\text{in}}}^{\mathrm{\text{n}}}$ can be expressed as follows:

$\begin{array}{llll}{\omega }_{\text{in}}^{\text{n}}& =& {\text{ω}}_{\text{ie}}^{\text{n}}+{\text{ω}}_{\text{en}}^{\text{n}}& (14)\end{array}$

$\begin{array}{llll}\delta {\text{ω}}_{\text{in}}^{\text{n}}& =& \delta {\text{ω}}_{\text{ie}}^{\text{n}}+\delta {\text{ω}}_{\text{en}}^{\text{n}}& (15)\end{array}$

On the basis of these formulas, the INS attitude error and velocity error have been derived (Yan et al., 2008):

$\begin{array}{ll}\dot{\alpha }={\text{C}}_{\omega }^{\text{-}1}\left[\left(I\text{-}{\text{C}}_{\text{n}}^{\text{n}\prime }\right){\widehat{\omega }}_{\text{in}}^{\text{n}}+{\text{C}}_{\text{n}}^{\text{n}\prime }\delta {\omega }_{\text{in}}^{\text{n}}-{\text{C}}_{\text{b}}^{\text{n}\prime }\delta {\omega }_{\text{ib}}^{\text{b}}\right]& (16)\end{array}$

$\begin{array}{ll}\begin{array}{ll}\delta {\dot{\text{v}}}^{\text{n}}\hfill & =\left[\text{I}\text{-}{\left({\text{C}}_{\text{n}}^{\text{n}\prime }\right)}^{\text{T}}\right]{\text{C}}_{\text{b}}^{\text{n}\prime }{\widehat{\text{f}}}_{\text{sf}}^{\text{b}}+{\left({\text{C}}_{\text{n}}^{\text{n}\prime }\right)}^{\text{T}}{\text{C}}_{\text{b}}^{\text{n}\prime }\delta {\text{f}}_{\text{sf}}^{\text{b}}-\left(2\delta {\omega }_{\text{ie}}^{\text{n}}+\delta {\omega }_{\text{en}}^{\text{n}}\right)\hfill \\ \hfill & \times \left({\widehat{\text{v}}}^{\text{n}}-\delta {\text{v}}^{\text{n}}\right)-\left(2{\widehat{\omega }}_{\text{ie}}^{\text{n}}+{\widehat{\omega }}_{\text{en}}^{\text{n}}\right)\times \delta {\text{v}}^{\text{n}}+\delta {\text{g}}^{\text{n}}\hfill \end{array}& (17)\end{array}$

Where $\delta {\omega }_{\mathrm{\text{ib}}}^{\mathrm{\text{b}}}$ is the measurement error of gyroscope, $\delta {\text{ω}}_{\mathrm{\text{in}}}^{\mathrm{\text{n}}}$ is the calculation error of ${\text{ω}}_{\mathrm{\text{in}}}^{\mathrm{\text{n}}}$. $\delta f_{\mathrm{\text{sf}}}^{\mathrm{\text{b}}}$ is the measurement error of accelerometer. In addition, $\delta {\text{ω}}_{\mathrm{\text{ib}}}^{\mathrm{\text{b}}}$ are mainly consist of a constant bias ε^b and zero mean Gaussian white noise ${\text{w}}_{\mathrm{\text{g}}}^{\mathrm{\text{b}}}$. $\delta f_{\mathrm{\text{sf}}}^{\mathrm{\text{b}}}$ are mainly consist of a constant bias ∇^b and zero mean Gaussian white noise ${\text{w}}_{\mathrm{\text{a}}}^{\mathrm{\text{b}}}$. Besides, δgⁿcould be ignored. Hence, the INS error propagation model used in underwater environment is defined by the following equations:

$\begin{array}{ll}\{\begin{array}{l}\delta \dot{L}\text{=}\frac{\delta v_{\text{N}}^{\text{n}}}{R_{\text{M}}-h}+\delta h\frac{v_{\text{N}}^{\text{n}}}{{\left(R_{\text{M}}-h\right)}^2}\hfill \\ \delta \dot{\lambda }\text{=}\frac{\delta v_{\text{E}}^{\text{n}}}{R_{\text{N}}-h}\sec L+\delta L\frac{v_{\text{E}}^{\text{n}}}{R_{\text{N}}-h}\tan L\sec L+\delta h\frac{v_{\text{E}}^{\text{n}}\sec L}{{\left(R_{\text{N}}-h\right)}^2}\hfill \\ \delta \dot{h}\text{=}\delta V_{\text{D}}\hfill \\ \dot{\alpha }\text{=}{\text{C}}_{\omega }^{\text{-}1}\left[\left(\text{I}\text{-}{\text{C}}_{\text{n}}^{\text{n}\prime }\right){\widehat{\omega }}_{\text{in}}^{\text{n}}+{\text{C}}_{\text{n}}^{\text{n}\prime }\delta {\omega }_{\text{in}}^{\text{n}}-{\text{C}}_{\text{b}}^{\text{n}\prime }{\epsilon }^{\text{b}}\right]-{\text{C}}_{\omega }^{\text{-}1}{\text{C}}_{\text{b}}^{\text{n}\prime }{\text{w}}_{\text{g}}^{\text{b}}\hfill \\ \delta {\dot{v}}^{\text{n}}\text{=}\left[\text{-}{\left({\text{C}}_{\text{n}}^{\text{n}\prime }\right)}^{\text{T}}\right]C_{\text{b}}^{\text{n}\prime }{\widehat{f}}_{\text{sf}}^{\text{b}}+{\left({\text{C}}_{\text{n}}^{\text{n}\prime }\right)}^{\text{T}}{\text{C}}_{\text{b}}^{\text{n}\prime }{\nabla }^{\text{b}}-\left(2\delta {\omega }_{\text{ie}}^{\text{n}}+\delta {\omega }_{\text{en}}^{\text{n}}\right)\hfill \\ \times \left({\widehat{v}}^{\text{n}}-\delta {\text{v}}^{\text{n}}\right)-\left(2{\widehat{\omega }}_{\text{ie}}^{\text{n}}+{\widehat{\omega }}_{\text{en}}^{\text{n}}\right)\times \delta {\text{v}}^{\text{n}}+{\left({\text{C}}_{\text{n}}^{\text{n}\prime }\right)}^{\text{T}}{\text{C}}_{\text{b}}^{\text{n}\prime }{\text{w}}_{\text{a}}^{\text{b}}\hfill \\ {\dot{\epsilon }}^{\text{b}}\text{=}0\hfill \\ {\dot{\nabla }}^{\text{b}}\text{=}0\hfill \end{array}& (18)\end{array}$

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:

$\begin{array}{ll}{d}_{k-1,k}^{\text{INS}}=\frac{\left|\right|{\text{v}}_{k-1}^{\text{INS}}+{\text{v}}_k^{\text{INS}}\left|\right|}{2}·\Delta t& (19)\end{array}$

$\begin{array}{ll}{d}_{k-1,k}^{\text{real}}\approx \frac{\left|\right|{\text{v}}_{k-1}^{\text{real}}+{\text{v}}_k^{\text{real}}\left|\right|}{2}·\Delta t& (20)\end{array}$

$d_{k\mathrm{\text{-}}1,k}^{\mathrm{\text{INS}}}$ and $d_{k-1,k}^{\mathrm{\text{real}}}$ are, respectively, the INS-indicated distance and the actual distance between k-1th sampling point and kth sampling point. $v_k^{\mathrm{\text{INS}}}$ and $v_k^{\mathrm{\text{real}}}$ are the INS-indicated linear velocity and the actual linear velocity of kth sampling point. Δt is the sampling period of the discrete time system.

Then, the relationship between $d_{k\mathrm{\text{-}}1,k}^{\mathrm{\text{INS}}}$and $d_{k-1,k}^{\mathrm{\text{real}}}$ is derived:

$\begin{array}{ll}\frac{d_{k-1,k}^{\text{real}}}{d_{k-1,k}^{\text{INS}}}=\frac{\left|\right|{\text{v}}_{k-1}^{\text{real}}+{\text{v}}_k^{\text{real}}\left|\right|}{\left|\right|v_{k-1}^{\text{INS}}+v_k^{\text{INS}}\left|\right|}& (21)\end{array}$

Where ${\text{v}}_k^{\mathrm{\text{INS}}}$is described by an equation in the form:

$\begin{array}{ll}{v}_k^{\text{INS}}=v_k^{\text{real}}+\delta v_k& (22)\end{array}$

Here δv_k is the INS accumulated velocity error of kth sampling point. Accordingly, (21) is developed into the following form:

$\begin{array}{ll}\frac{d_{k-1,k}^{\text{real}}}{d_{k-1,k}^{\text{INS}}}=\frac{\left|\right|{\text{v}}_{k-1}^{\text{INS}}+{\text{v}}_k^{\text{INS}}-\delta {\text{v}}_{k\text{-}1}-\delta {\text{v}}_k\left|\right|}{\left|\right|{\text{v}}_{k-1}^{\text{INS}}+{\text{v}}_k^{\text{INS}}\left|\right|}& (23)\end{array}$

Denoting with N the number of sampling points on matching sequence, the constraint of scale transform has been derived:

$\begin{array}{ll}S=\frac{{\displaystyle \sum _{k=2}^N}{d}_{k-1,k}^{\text{real}}}{{\displaystyle \sum _{k=2}^N}{d}_{k-1,k}^{\text{INS}}}=\frac{{\displaystyle \sum _{k=2}^N}\left|\right|{\text{v}}_{k-1}^{\text{INS}}+{\text{v}}_k^{\text{INS}}-\delta {\text{v}}_{k\text{-}1}-\delta {\text{v}}_k\left|\right|}{{\displaystyle \sum _{k=2}^N}\left|\right|{\text{v}}_{k-1}^{\text{INS}}+{\text{v}}_k^{\text{INS}}\left|\right|}& (24)\end{array}$

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:

$\begin{array}{ll}f(\text{x})=\frac{1}{N}{\displaystyle \sum _{k=1}^N}|g_k^{\text{obs}}-g_k^{\text{x}}|& (25)\end{array}$

Where x indicates a sequence of several consecutive position vectors. $g_k^{\mathrm{\text{obs}}}$ is the measured gravity anomaly value of kth sampling point and $g_k^{\text{x}}$ 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):

$\begin{array}{ll}fit(\text{x})=\frac{1}{1+f(\text{x})}& (26)\end{array}$

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:

$\begin{array}{ll}{x}_{{}_{i,1}}^L=x_{min}^L+rand(0,1)(x_{max}^L-x_{min}^L)& (27)\end{array}$

$\begin{array}{ll}{x}_{{}_{i,1}}^{\lambda }=x_{min}^{\lambda }+rand(0,1)(x_{max}^{\lambda }-x_{min}^{\lambda })& (28)\end{array}$

Where $x_{{}_{i,1}}^L$ and $x_{{}_{i,1}}^{\lambda }$ 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_{max}^L$ and $x_{min}^L$ are, respectively, the upper bound and the lower bound of latitude in search scope. $x_{max}^{\lambda }$ and $x_{min}^{\lambda }$ are, respectively, the upper bound and the lower bound of longitude in search scope.

Let $S_i^{\mathrm{\text{ini}}}$ be the scale factor used for initialization sequence i. The value of $S_i^{\mathrm{\text{ini}}}$ is calculated by (29):

$\begin{array}{ll}{S}_i^{\text{ini}}=\{\begin{array}{cc}rand(1,S),& S\ge 1\\ rand(S,1),& S<1\end{array}& (29)\end{array}$

The expression of S is obtained from (24). The rotation angle used for initialization sequence i can be calculated by the following equation:

$\begin{array}{ll}{\beta }_i^{\text{ini}}=rand({\beta }_{min}^{\text{ini}},{\beta }_{max}^{\text{ini}})& (30)\end{array}$

Where ${\beta }_{min}^{\mathrm{\text{ini}}}$ and ${\beta }_{max}^{\mathrm{\text{ini}}}$ 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:

$\begin{array}{ll}{\text{R}}_i^{\text{ini}}=\left[\begin{array}{cc}cos{\beta }_i^{\text{ini}}& -sin{\beta }_i^{\text{ini}}\\ sin{\beta }_i^{\text{ini}}& cos{\beta }_i^{\text{ini}}\end{array}\right]& (31)\end{array}$

Let ${\text{X}}^{\mathrm{\text{INS}}}=\left\{{\text{x}}_1^{\mathrm{\text{INS}}},{\text{x}}_2^{\mathrm{\text{INS}}},\dots ,{\text{x}}_N^{\mathrm{\text{INS}}}\right\}$ be the INS-indicated trajectory and ${\text{x}}_i^{\mathrm{\text{ini}}}=\left\{{\text{x}}_{i,1}^{\mathrm{\text{ini}}},{\text{x}}_{i,2}^{\mathrm{\text{ini}}},\dots ,{\text{x}}_{i,N}^{\mathrm{\text{ini}}}\right\}$ be the ith initialization sequence. After above mentioned transformations have been performed, the initial swarm is obtained:

$\begin{array}{ll}{\text{X}}_I^{\text{INS}\prime }=S_i^{\text{ini}}{\text{R}}_i^{\text{ini}}{\text{X}}^{\text{INS}}& (32)\end{array}$

$\begin{array}{ll}{\text{T}}_i^{\text{ini}}={\text{x}}_{i,1}^{\text{ini}}-{\text{x}}_1^{\text{INS}\prime }& (33)\end{array}$

$\begin{array}{ll}{\text{X}}_i^{\text{ini}}={\text{X}}_i^{INS\prime }\text{+}{\text{T}}_i^{\text{ini}}& (34)\end{array}$

Where ${\text{X}}_i^{\text{INS}\prime }$ is the intermediate sequence, ${\text{x}}_i^{\text{INS}\prime }$ is the first point in ${\text{X}}_i^{\text{INS}\prime }$. Additional translation vector need to be applied on generated trajectories if they are out of range. Let $ini{t}_{min}^L$ and $ini{t}_{max}^L$ be the maximum and minimum latitude of initial trajectory. $ini{t}_{min}^{\lambda }$ and $ini{t}_{max}^{\lambda }$ are the maximum and minimum longitude of initial trajectory. T = {T^L, T^λ}, the translation vector, can be expressed as:

$\begin{array}{ll}{T}^L=\{\begin{array}{ll}{x}_{\min }^L-ini{t}_{\min }^L,\hfill & ini{t}_{\min }^L<x_{\min }^L\hfill \\ x_{\max }^L-ini{t}_{\max }^L,\hfill & ini{t}_{\max }^L>x_{\max }^L\hfill \\ 0,\hfill & \text{other}\hfill \end{array}& (35)\end{array}$

$\begin{array}{ll}{T}^{\lambda }=\{\begin{array}{ll}{x}_{\min }^{\text{λ}}-ini{t}_{\min }^{\text{λ}},\hfill & ini{t}_{\min }^{\text{λ}}<x_{\min }^{\text{λ}}\hfill \\ x_{\max }^{\text{λ}}-ini{t}_{\max }^{\text{λ}},\hfill & ini{t}_{\max }^{\text{λ}}>x_{\max }^{\text{λ}}\hfill \\ 0,\hfill & \text{other}\hfill \end{array}& (36)\end{array}$

Employed Bee Phase

With the initialization complete, an employed bee updates its food source in the neighborhood. Suppose ${\text{X}}_i^{\mathrm{\text{ini}}}$ is the sequence to be updated, ${\text{X}}_j^{\mathrm{\text{ini}}}=\left\{{\text{x}}_{j,1}^{\mathrm{\text{ini}}},{\text{x}}_{j,2}^{\mathrm{\text{ini}}},\dots ,{\text{x}}_{j,N}^{\mathrm{\text{ini}}}\right\}$ is selected randomly in the neighborhood of ${\text{x}}_i^{\mathrm{\text{ini}}}$, j ∈ {1, 2, ⋯ , i − 1, i + 1, ⋯ , SN} and SN is the size of population. The evolution equation is derived through the relation between ${\text{x}}_i^{\mathrm{\text{ini}}}$ and ${\text{x}}_j^{\mathrm{\text{ini}}}$.

Denoting with Q the covariance matrix:

$\begin{array}{ll}\text{Q}=\left[\begin{array}{cc}{Q}_{11}& Q_{12}\\ Q_{21}& Q_{22}\end{array}\right]={\displaystyle \sum _{k=1}^N}\left({\text{x}}_{i,k}^{\text{ini}}-{\tilde{\text{x}}}_i^{\text{ini}}\right){\left({\text{x}}_{j,k}^{\text{ini}}-{\tilde{\text{x}}}_j^{\text{ini}}\right)}^{\text{T}}& (37)\end{array}$

Where ${\tilde{\text{x}}}_i^{\mathrm{\text{ini}}}$ and ${\tilde{\text{x}}}_j^{\mathrm{\text{ini}}}$ are, respectively, the average value of all sampling points in ${\text{X}}_i^{\mathrm{\text{ini}}}$ and ${\text{X}}_j^{\mathrm{\text{ini}}}$. The eigenvalues of Q and the rotation angle from ${\text{X}}_i^{\mathrm{\text{ini}}}$ to ${\text{X}}_j^{\mathrm{\text{ini}}}$ are calculated through quaternion algorithm (Berthold, 1987):

$\begin{array}{lll}{\lambda }_{1,2}& =& \pm {\left[{\left(Q_{11}+Q_{22}\right)}^2+{\left(Q_{21}-Q_{12}\right)}^2\right]}^{1/2}\\ {\lambda }_{3,4}& =& \pm {\left[{\left(Q_{11}-Q_{22}\right)}^2+{\left(Q_{21}+Q_{12}\right)}^2\right]}^{1/2}& (38)\end{array}$

$\begin{array}{llll}tg(\frac{\tau }{2})& =& \left(Q_{11}+Q_{22}-{\lambda }_{\text{m}}\right)/\left(Q_{12}-Q_{21}\right)& (39)\end{array}$

Where λ_μ (μ = {1, 2, 3, 4}) are four eigenvalues of Q. λ_m is the maximum eigenvalue. τ is the rotation angle from ${\text{X}}_i^{\mathrm{\text{ini}}}$ to ${\text{X}}_j^{\mathrm{\text{ini}}}$. Then, the rotation angle used for ${\text{X}}_i^{\mathrm{\text{ini}}}$ during employed bee phase is calculated by the following equations:

$\begin{array}{ll}{\beta }_i^{\text{em}}=rand(-\left|\tau \right|,\left|\tau \right|)& (40)\end{array}$

Notice that the angle between ${\text{X}}_i^{\mathrm{\text{ini}}}$ and X^INS after rotation should be in the range $({\beta }_{min}^{\mathrm{\text{ini}}},{\beta }_{max}^{\mathrm{\text{ini}}})$. Denoting with η be the rotation angle from ${\text{X}}_i^{\mathrm{\text{ini}}}$ to X^INS, ${\beta }_i^{\mathrm{\text{em}}}$ should meet the following in equation:

$\begin{array}{ll}{\beta }_{min}^{\text{ini}}+\eta <{\beta }_i^{\text{em}}<{\beta }_{max}^{\text{ini}}+\eta & (41)\end{array}$

Then, the following expression is obtained by (40) and (41):

$\begin{array}{ll}{\beta }_i^{\text{em}}=rand\left(max(-|\tau |,{\beta }_{min}^{\text{ini}}+\eta ),min(|\tau |,{\beta }_{max}^{\text{ini}}+\eta )\right)& (42)\end{array}$

Thus, the rotation matrix used for ${\text{X}}_i^{\mathrm{\text{ini}}}$ during employed bee phase is calculated by (43):

$\begin{array}{ll}{\text{R}}_i^{\text{em}}=\left[\begin{array}{cc}cos{\beta }_i^{\text{em}}& -sin{\beta }_i^{\text{em}}\\ sin{\beta }_i^{\text{em}}& cos{\beta }_i^{\text{em}}\end{array}\right]& (43)\end{array}$

Let $L_i^{\mathrm{\text{ini}}}$ and $L_j^{\mathrm{\text{ini}}}$ be the length of ${\text{X}}_i^{\mathrm{\text{ini}}}$ and ${\text{X}}_j^{\mathrm{\text{ini}}}$. L^ins is the length of INS-indicated trajectory. The scale factor used for ${\text{X}}_i^{\mathrm{\text{ini}}}$ during employed bee phase is calculated by the following equations:

$\begin{array}{ll}{S}_i^{\text{em}}=rand(min(\frac{{\displaystyle L_j^{\text{ini}}}}{{\displaystyle L_i^{\text{ini}}}},\frac{{\displaystyle L_i^{\text{ini}}}}{{\displaystyle L_j^{\text{ini}}}}),max(\frac{{\displaystyle L_j^{\text{ini}}}}{{\displaystyle L_i^{\text{ini}}}},\frac{{\displaystyle L_i^{\text{ini}}}}{{\displaystyle L_j^{\text{ini}}}}))& (44)\end{array}$

Notice that the scale factor between ${\text{X}}_i^{\mathrm{\text{ini}}}$ and X^INS after scaling transformation should meet the range on the right side of (29):

$\begin{array}{ll}\{\begin{array}{l}1\cdot L^{\text{ins}}<L_i^{\text{ini}}·S_i^{\text{em}}<S·L^{\text{ins}},S\ge 1\hfill \\ S\cdot L^{\text{ins}}<L_i^{\text{ini}}·S_i^{\text{em}}<1·L^{\text{ins}},S<1\hfill \end{array}& (45)\end{array}$

Denoting with $S_{min}^{\mathrm{\text{em}}}$ and $S_{max}^{\mathrm{\text{em}}}$ the lower bound and the upper bound of $S_i^{\mathrm{\text{em}}}$. According to (44) and (45), $S_i^{\mathrm{\text{em}}}$ is updated by (46):

$\begin{array}{llll}{S}_i^{\text{em}}& =& rand(S_{min}^{\text{em}},S_{m\text{ax}}^{\text{em}})& (46)\end{array}$

$\begin{array}{ll}\begin{array}{l}\begin{array}{ll}{S}_{\min }^{\text{em}}=\hfill & \{\begin{array}{l}\max (min(\frac{L_j^{\text{ini}}}{L_i^{\text{ini}}},\frac{L_i^{\text{ini}}}{L_j^{\text{ini}}}),\frac{L^{\text{ins}}}{L_i^{\text{ini}}}),S\ge 1\hfill \\ \max (min(\frac{L_j^{\text{ini}}}{L_i^{\text{ini}}},\frac{L_i^{\text{ini}}}{L_j^{\text{ini}}}),S·\frac{L^{\text{ins}}}{L_i^{\text{ini}}}),S<1\hfill \end{array}\hfill \end{array}\hfill \\ \begin{array}{ll}{S}_{\max }^{\text{em}}=\hfill & \{\begin{array}{l}\min (\max (\frac{L_j^{\text{ini}}}{L_i^{\text{ini}}},\frac{L_i^{\text{ini}}}{L_j^{\text{ini}}}),S·\frac{L^{\text{ins}}}{L_i^{\text{ini}}}),S\ge 1\hfill \\ \min (\max (\frac{L_j^{\text{ini}}}{L_i^{\text{ini}}},\frac{L_i^{\text{ini}}}{L_j^{\text{ini}}}),·\frac{L^{\text{ins}}}{L_i^{\text{ini}}}),S<1\hfill \end{array}\hfill \end{array}\hfill \end{array}& (47)\end{array}$

${\text{X}}_i^{\text{ini}}$ is converted to ${\text{X}}_i^{\text{ini}\prime }$ after rotation transformation and scaling transformation have been performed:

$\begin{array}{ll}{X}_i^{\text{ini}\prime }=S_i^{\text{em}}{R}_i^{\text{em}}{X}_i^{\text{ini}}& (48)\end{array}$

Let ${\tilde{\text{x}}}_i^{\text{ini}\prime }$ be the average value of all sampling points in ${\text{X}}_i^{\text{ini}\prime }$ the translation vector is calculated by (49):

$\begin{array}{ll}{\text{T}}_i^{\text{em}}=({\tilde{\text{x}}}_i^{\text{ini}}-{\tilde{\text{x}}}_i^{\text{ini}\prime })·rand(-1,1)& (49)\end{array}$

Then, the employed bees update their sources according to the following equation:

$\begin{array}{ll}{\text{X}}_i^{\text{em}}={\text{X}}_i^{\text{ini}\prime }+{\text{T}}_i^{\text{em}}& (50)\end{array}$

If the fitness value of ${\text{X}}_i^{\text{em}}$ is larger than the fitness value of ${\text{X}}_i^{\mathrm{\text{ini}}}$, ${\text{X}}_i^{\mathrm{\text{ini}}}$ will be replaced by ${\text{X}}_i^{\mathrm{\text{em}}}$. 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 ${\text{X}}_i^{\mathrm{\text{em}}}$ is the sequence selected by onlooker bee, the rotation matrix ${\text{R}}_i^{\mathrm{\text{on}}}$, the scale factor $S_i^{\mathrm{\text{on}}}$, and the translation vector ${\text{T}}_i^{\mathrm{\text{on}}}$ used on ${\text{X}}_i^{\mathrm{\text{em}}}$ during onlooker bee phase can be calculated by (43), (46), and (49). Thus, the onlooker bees update their sources according to the following equation:

$\begin{array}{ll}{\text{X}}_i^{\text{on}}=S_i^{\text{on}}{\text{R}}_i^{\text{on}}{\text{X}}_i^{\text{em}}+{\text{T}}_i^{\text{on}}& (51)\end{array}$

If the fitness value of ${\text{X}}_i^{\mathrm{\text{on}}}$ is larger than the fitness value of ${\text{X}}_i^{\mathrm{\text{em}}}$, ${\text{X}}_i^{\mathrm{\text{em}}}$ will be replaced by ${\text{X}}_i^{\mathrm{\text{on}}}$. 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

Simulation and Analysis

Simulation Parameters

In order to test the feasibility of our approach, numerous simulation experiments have been performed. An INS indicated trajectory start from (156°E, 20°N) is generated and the INS relevant parameters are listed in Table 1. The INS position error is shown in 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.

TABLE 1

Table 1. Simulation condition of INS.

FIGURE 1

Figure 1. The INS position error.

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 2

Figure 2. 3-D gravity anomaly base map.

TABLE 2

Table 2. Parameters of gravity anomaly base map.

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, ${\beta }_{min}^{\mathrm{\text{ini}}}$ and ${\beta }_{m\mathrm{\text{ax}}}^{\mathrm{\text{ini}}}$ 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.

FIGURE 3

Figure 3. The INS-indicated segment angle error.

TABLE 3

Table 3. Configuration of the proposed algorithm.

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′.

FIGURE 4

Figure 4. Position errors of basic ABC.

FIGURE 5

Figure 5. Position errors of affine transformation based ABC.

TABLE 4

Table 4. Comparison of 50 Mont Carlo simulation results between basic ABC and affine transformation based ABC.

Comparison Between ICCP, Improved-ABC and Affine Transformation Based ABC

Due to ICCP is a widely used gravity matching algorithm and improved-ABC algorithm (Gao et al., 2014) is presented to tackle the similar problem. We use these algorithms as references, comparing the results of ICCP algorithm and improved-ABC algorithm with the proposed algorithm.

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.

FIGURE 6

Figure 6. Matched trajectory.

FIGURE 7

Figure 7. The average error of longitude and latitude.

TABLE 5

Table 5. Statistical results of the matching errors.

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.

FIGURE 8

Figure 8. Matching results of the whole trajectory.

TABLE 6

Table 6. Statistical results within 16 h.

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).

Conflict of Interest Statement

The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.

Supplementary Material

The Supplementary Material for this article can be found online at: https://www.frontiersin.org/articles/10.3389/fnbot.2019.00019/full#supplementary-material

Supplementary Data Sheet 1. Gravity base map and the INS data.

References

Abrao, T. (2013). Search Algorithms for Engineering Optimization: Chapter 8- Application of Harmony Search Algorithm in Power Engineering, Chapter Book-Chapter 8. Crovatia: InTech Open Access Publishing.

Affleck, C. A., and Jircitano, A. (1990). “Passive gravity gradiometer navigation system,” in Proceedings of IEEE Position Location and Navigation Symposium (Las Vegas, NV), 60–66.

Google Scholar

Akay, B., and Karaboga, D. (2012). A modified artificial bee colony algorithm for real-parameter optimization. Inform. Sci. 192, 120–142. doi: 10.1016/j.ins.2010.07.015

CrossRef Full Text | Google Scholar

Akbari, R., Hedayatzadeh, R., Ziarati, K., and Hassanizadeh, B. (2012). A multi-objective artificial bee colony algorithm. Swarm Evol. Comput. 2, 39–52. doi: 10.1016/j.swevo.2011.08.001

CrossRef Full Text | Google Scholar

Allotta, B., Caiti, A., Costanzi, R., Fanelli, F., Fenucci, D., and Meli, E., et al. (2016). A new AUV navigation system exploiting unscented Kalman filter. Ocean Eng. 113, 121–132. doi: 10.1016/j.oceaneng.2015.12.058

CrossRef Full Text | Google Scholar

Berthold, K. P. H. (1987). Closed-form solution of absolute orientation using unit quaternions. J. Opt. Soc. Am. A 4, 629–642.

Google Scholar

Bishop, G. C. (2002). Gravitational field maps and navigational errors [unmanned underwater vehicles]. IEEE J. Ocean. Eng. 27, 726–737. doi: 10.1109/JOE.2002.1040954

CrossRef Full Text | Google Scholar

Canciani, A., and Raquet, J. (2017). Airborne magnetic anomaly navigation. IEEE Trans. Aerosp. Electron. Syst. 53, 67–80. doi: 10.1109/TAES.2017.2649238

CrossRef Full Text | Google Scholar

Claus, B., and Bachmayer, R. (2015). Terrain-aided navigation for an underwater glider. J. Field Robot. 32, 935–951. doi: 10.1002/rob.21563

CrossRef Full Text | Google Scholar

Copp, B., and Subbarao, K. (2015). Nonlinear adaptive filtering in terrain-referenced navigation. IEEE Trans. Aerosp. Electron. Syst. 51, 3461–3469. doi: 10.1109/TAES.2015.140826

CrossRef Full Text | Google Scholar

Fecher, T., Pail, R., and Gruber, T. (2017). Goco05c: a new combined gravity field model based on full normal equations and regionally varying weighting. Surv. Geophys. 38, 571–590. doi: 10.1007/s10712-016-9406-y

CrossRef Full Text | Google Scholar

Gao, W., Zhao, B., Zhou, G., Wang, Q., and Yu, C. (2014). Improved artificial bee colony algorithm based gravity matching navigation method. Sensors 14, 12968–12989. doi: 10.3390/s140712968

PubMed Abstract | CrossRef Full Text | Google Scholar

Han, Y., Wang, B., Deng, Z., and Fu, M. (2016). An improved TERCOM-based algorithm for gravity-aided navigation. IEEE Sens. J. 16, 2537–2544. doi: 10.1109/JSEN.2016.2518686

CrossRef Full Text | Google Scholar

Hegrenaes, O., and Hallingstad, O. (2011). Model-aided INS with sea current estimation for robust underwater navigation. IEEE J. Ocean. Eng. 36, 316–337. doi: 10.1109/JOE.2010.2100470

CrossRef Full Text | Google Scholar

Hidalgo-Paniagua, A., Vega-Rodríguez, M., and Ferruz, J. (2016). Applying the MOVNS (multi-objective variable neighborhood search) algorithm to solve the path planning problem in mobile robotics. Expert Syst. Appl. 58, 20–35. doi: 10.1016/j.eswa.2016.03.035

CrossRef Full Text | Google Scholar

Kamgarparsi, B., and Kamgarparsi, B. (1999). Vehicle localization on gravity maps. Proc. SPIE. 3693, 182–191.

Google Scholar

Karaboga, D., and Basturk, B. (2007). A powerful and efficient algorithm for numerical function optimization: artificial bee colony (ABC) algorithm. J. Glob. Optimization 39, 459–471. doi: 10.1007/s10898-007-9149-x

CrossRef Full Text | Google Scholar

Kim, T., Kim, J., and Byun, S. W. (2018). A comparison of nonlinear filter algorithms for terrain-referenced underwater navigation. Int. J. Control Autom. Syst. 16, 2977–2989. doi: 10.1007/s12555-017-0504-5

CrossRef Full Text | Google Scholar

Kim, Y., Hong, K., and Bang, H. (2018). Utilizing out-of-sequence measurement for ambiguous update in particle filtering. IEEE Trans. Aerosp. Electron. Syst. 54, 493–501. doi: 10.1109/TAES.2017.2741878

CrossRef Full Text | Google Scholar

Lee, J., and Kim, D. W. (2016). An effective initialization method for genetic algorithm-based robot path planning using a directed acyclic graph. Inf. Sci. (NY). 332, 1–18. doi: 10.1016/j.ins.2015.11.004

CrossRef Full Text | Google Scholar

Li, H., Liu, M., and Zhang, F. (2017). Geomagnetic navigation of autonomous underwater vehicle based on multi-objective evolutionary algorithm. Front. Neurorobot. 11, 1–8. doi: 10.3389/fnbot.2017.00034

PubMed Abstract | CrossRef Full Text | Google Scholar

Li, J., Pan, Q., and Duan, P. (2016). An improved artificial bee colony algorithm for solving hybrid flexible flowshop with dynamic operation skipping. IEEE Trans. Cybern. 46, 1311–1324. doi: 10.1109/TCYB.2015.2444383

PubMed Abstract | CrossRef Full Text | Google Scholar

Li, T., Zhao, D., and Huang, Z. (2013). A wavelet-based grey particle filter for self-estimating the trajectory of manoeuvring autonomous underwater vehicle. Trans. Inst. Meas. Control 36, 321–335. doi: 10.1177/0142331213500981

CrossRef Full Text | Google Scholar

Paull, L., Saeedi, S., Seto, M., and Li, H. (2013). AUV navigation and localization: a review. IEEE J. Ocean. Eng. 2013 39, 131–149. doi: 10.1109/JOE.2013.2278891

CrossRef Full Text | Google Scholar

Quan, W., and Fang, J. (2010). A star recognition method based on the adaptive ant colony algorithm for star sensors. Sensors 10, 1955–1966. doi: 10.3390/s100301955

PubMed Abstract | CrossRef Full Text | Google Scholar

Rice, H., Kelmenson, S., and Mendelsohn, L. (2004). “Geophysical navigation technologies and applications,” in Proceedings of IEEE Position Location and Navigation Symposium (Monterey, CA), 618–624.

Google Scholar

Sag, T., and Çunkaş, M. (2015). Color image segmentation based on multiobjective artificial bee colony optimization. Appl. Soft Comput. 34, 389–401. doi: 10.1016/j.asoc.2015.05.016

CrossRef Full Text | Google Scholar

Simanek, J., Reinstein, M., and Kubelka, V. (2015). Evaluation of the EKF-based estimation architectures for data fusion in mobile robots. IEEE ASME Trans. Mech. 20, 985–990. doi: 10.1109/TMECH.2014.2311416

CrossRef Full Text | Google Scholar

Song, Z., Zhang, J., Zhu, W., and Xi, X. (2016). The vector matching method in geomagnetic aiding navigation. Sensors 16:1120. doi: 10.3390/s16071120

PubMed Abstract | CrossRef Full Text | Google Scholar

Teymourian, E., Kayvanfar, V., Komaki, G. H. M., and Zandieh, M. (2016). Enhanced intelligent water drops and cuckoo search algorithms for solving the capacitated vehicle routing problem. Inf. Sci. (NY). 334, 354–378. doi: 10.1016/j.ins.2015.11.036

CrossRef Full Text | Google Scholar

Tong, Y., Bian, S., Jiang, D., and Xiao, S. (2011). Gravity matching simulation of real-time iccp algorithm. J. Chin. Inertial Technol. 19, 340–343.

Google Scholar

Wang, X., Su, M., Liu, P., and Ding, S. (2013). Application of improved ICCP algorithm in gravity matching aided navigation. Sci. Surv. Mapp. 207. 38, 36–39

Google Scholar

Wu, L., Wang, H., Chai, H., Hsu, H., and Wang, Y. (2015). Research on the relative positions-constrained pattern matching method for underwater gravity-aided inertial navigation. J. Navigation 68, 937–950. doi: 10.1017/S0373463315000235

CrossRef Full Text | Google Scholar

Wu, L., Wang, H., Chai, H., Zhang, L., Hsu, H., and Wang, Y. (2017). Performance evaluation and analysis for gravity matching aided navigation. Sensors 17:769. doi: 10.3390/s17040769

PubMed Abstract | CrossRef Full Text | Google Scholar

Xu, X., Wu, J., Xu, S., Wang, L., and Li, P. (2014). ICCP algorithm for underwater terrain matching navigation based on affine correction. J. Chin. Inertial Technol. 22, 362–367. doi: 10.13695/j.cnki.12-1222/o3.2014.03.016

CrossRef Full Text

Yan, G., Yan, W., and Xu, D. (2008). Application of simplified UKF in SINS initial alignment for large misalignment angles. J. Chin. Inertial Technol. 16, 253–264. doi: 10.13695/j.cnki.12-1222/o3.2008.03.006

CrossRef Full Text

Yildiz, A. R. (2013). Comparison of evolutionary-based optimization algorithms for structural design optimization. Eng. Appl. Artif. Intell. 26, 327–333. doi: 10.1016/j.engappai.2012.05.014

CrossRef Full Text | Google Scholar

Zhao, L., Gao, N., Huang, B., Wang, Q., and Zhou, J. (2014). A novel terrain-aided navigation algorithm combined with the TERCOM algorithm and particle filter. IEEE Sens. J. 15, 1124–1131. doi: 10.1109/JSEN.2014.2360916

CrossRef Full Text | Google Scholar

Zhu, Z., Guo, Y., and Yang, Z. (2015). Study on initial gravity map matching technique based on triangle constraint model. J. Navigation 69, 353–372. doi: 10.1017/S0373463315000661

CrossRef Full Text | Google Scholar