1. INTRODUCTION
Accurate positioning of vehicles is essential for many underwater applications. Terrain Relative Navigation (TRN) can provide a drift-free accurate positioning result, which makes it a good compliment to INS for underwater navigation.
TRN technology has been studied since the 1970s. Earlier methods such as TERCOM (Golden, Reference Golden1980) and SITAN (Hostetler and Andreas, Reference Hostetler and Andreas1983) measured the host vehicle's vertical distance from the bottom at single points along a track. As far as UTRN is concerned, many variants have been successfully developed for underwater vehicles, most of which have reported high accuracy by using a combination of multi-beam bathymetric systems (MBS) and high-accuracy underwater terrain maps. For example, the Bayesian method was applied to UTRN (Bergman et al., Reference Bergman, Ljung and Gustafsson1999; Karlsson and Gustafsson, Reference Karlsson and Gustafsson2003, Reference Karlsson and Gustafsson2006; Fairfield et al., Reference Fairfield and Wettergreen2008; Meduna et al., Reference Meduna, Rock and McEwen2008, Reference Meduna, Rock and McEwen2010; Anonsen, Reference Anonsen and Hallingstad2006, Reference Anonsen2010; Donovan, Reference Donovan2012). On the other hand, Nygren and Jansson (Reference Nygren and Jansson2004) proposed a method that measures seabed topography with a large number of sonar beams over a large sea bottom area. By simultaneously using a large number of measurements in one correlation process, the probability density function (PDF) of the position exhibits Gaussian properties, and thus the Kalman filter (KF) can be utilised for UTRN.
All of the above methods are based on the same prerequisite that the stochastic model is accurate to some degree in UTRN. However, outliers arise naturally in underwater terrain data, mainly in terrain measurements and the terrain model (terrain map). The sources of outliers in terrain measurements include hardware discontinuities, poor survey environment, shoals of fish and suspended objects in the water, which could cause a measurement error of up to several metres. Meanwhile, outliers in the terrain map occur from time to time. The possible sources of invalid terrain map data include the presence of significant map errors due to poor data collection. Moreover, even where the collected data is accurate, the terrain of the seafloor could change subsequently, due to erosion/deposit, sediment gravity flow, shifting sand dunes or even sunken boats. An example (Meduna, Reference Meduna2011) illustrated that the terrain can change greatly over a period of six months, the maximum change amounted to 10 m in certain regions. In the presence of all these outliers, the standard method may cause the solution to be heavily biased or even divergent. Therefore, a robust method is needed to overcome these outliers in real-time.
Several methods have been proposed in the literature to deal with outliers in UTRN. Gustafsson (Reference Gustafsson2010) pointed out that outliers in observations could be handled by dithering. Yet the degree of dithering is difficult to define to make an optimization between the efficiency and the robustness. In Nygren and Jansson's method (Reference Nygren and Jansson2004), the influence of occasional outliers is mitigated by large numbers of measurements in a single observation epoch, but when the proportion of outlier increases, the method yields unreliable results. To detect outliers in measurements, Lin et al. (Reference Lin, Yan, Liu and Tong2008) proposed an offline outlier identification procedure by wavelet analysis. However, in this method it is difficult to choose the threshold value for outlier identification that can balance the accuracy of identification and the rate of missing good observations. Hence, a real-time algorithm for UTRN is needed that can attain high efficiency as well as robustness.
This paper addresses outliers in UTRN. Enabling robust UTRN over the gross errors can substantially increase its applicability. A robust terrain-matching algorithm is developed to deal with outliers in a stochastic model.
2. THE PRINCIPLE OF UTRN
The UTRN can be modelled by the following equation pair:
$$\; x_t = x_{t - 1} + u_t + r_t $$
$$y_t = h(x_t ) + e_t $$where the vehicle position state is represented as:
$$x_t = [x_t^{North}, x_t^{East} ]$$In Equation (1), that models the state propagation, u t is the change in vehicle state measured by INS, and r t∼N(0,(σ r)2) is the process noise determined by the inertial drift rate and distance travelled. The measurement update step is given by Equation (2), where y t is the terrain measurement given by sonar, h(x t) is the terrain depth corresponding to x t according to the terrain model h(·), and e t∼N(0,(σ e)2) represents the noise of depth measurement. The noise e t consists of two parts, which are the noise of measurement e tsurvey∼N(0,(σ survey)2) and the noise of the map model, e tmap∼N(0,(σ map)2).
Assuming that the sonar measurement noise is uncorrelated with the map error and errors between beams are independent, the probability of terrain measurement y t, based on vehicle state x t, is given by:
$$L(y_t {\rm |}x_t ) = \displaystyle{1 \over {\sqrt {(2\pi \sigma _e^2 )^N}}} {\rm exp}\left( { - \displaystyle{1 \over {2\sigma _e^2}} \mathop \sum \limits_{k = 1}^N \; (y_t^k - h^k (x_t ))^2} \right)$$
$$\sigma _e^2 = \sigma _{survey}^2 + \sigma _{map}^2 {\rm \; \; \; \;} $$where N is the number of terrain measurements.
3. ROBUST ESTIMATION THEORY
Robust estimation theory defines outliers as the gross errors that cannot be described by a predefined distribution F (Huber, Reference Huber2011). The distribution of data contaminated by outliers is modelled by G=(1−ε)F+εH, where H is the distribution function of the outlier and ε is the level of the contamination. To access the robustness of a given estimator T(G), the breakdown point (BP) ε* is used (Hampel et al., Reference Hampel, Ronchetti, Rousseeuw and Stahel2011). The breakdown point ε* is defined as the largest fraction of contamination ε that an estimator can tolerate. The larger the breakdown point, more robust the estimator. Meanwhile, to measure the local sensitivity of an estimator T(G) to an arbitrary infinitesimal contamination Δr, the influence function is given as:
$$IF(r,F) = \mathop {\lim} \limits_{\varepsilon \to 0} \displaystyle{{T((1 - \varepsilon )F + \varepsilon \Delta _r ) - T(F)} \over \varepsilon} $$In contrast to the robustness measurements with regard to breakdown point and influence function, the asymptotically efficiency of an estimator T(G) is defined to access its efficiency when no outliers occur in the data. A trade-off exists between the breakdown point of an estimator and its asymptotic efficiency (Gandhi and Mili, Reference Gandhi and Mili2010). For example, the sample median achieves the highest breakdown point of 0·5, but its statistical efficiency is only 64% at the Gaussian distribution. Therefore, this robustness-efficiency trade-off needs to be suitably addressed when devising the solution to an estimation problem. In this paper, an estimate method (Gervini and Yohai, Reference Gervini and Yohai2002) that can simultaneously attain both high BP and high efficiency is applied.
4. ROBUST CORRELATION MATCHING
In UTRN, when the noise is described as Equation (5), the MLE 
$\hat x_{ML} (y_t )$ is given as:
$$\hat x_{ML} (y_t ) = \arg \mathop {\max} \limits_x L(y_t {\rm |}x_t ) = \arg \mathop {\min} \limits_x \mathop \sum \limits_{i = 1}^N (y_t^i - h(x_t )^i )^2 /\sigma _e^2 $$ It is obvious that in this situation, the MLE coincides with the Least Square Estimation 
$\hat x_{LSE} $, which is acquired by minimizing the score function:
$$J(x) = \mathop \sum \limits_{i = 1}^N (r^i )^2 $$where ri=y ti−h(x t)i is the residual of the ith measurement. In the rest of this paper, this kind of matching method is referred to as the ordinary matching method.
When outliers exist in the data, the matching result 
$\hat x_{LSE} $ could be seriously biased. Therefore, a robust correlation matching method is needed to handle the probable outliers. In this paper, we use the robust and efficient weighted least square estimator (REWLSE) (Gervini and Yohai, Reference Gervini and Yohai2002) to deal with outliers in terrain data. The method treats an observation as an outlier not only when its residual is greater than a given threshold value, but also when its value is sufficiently larger than the corresponding order statistic. By doing this, the method can achieve full efficiency at the normal distribution and at the same time have a high BP.
In the robust matching algorithm, a least median square estimation (LMSE) is executed firstly to provide an initial estimation x 0. Different from LSE, LMSE estimates the position based on the least median square (LMS) regulation:
$$\hat x_0 = \arg \mathop {\min} \limits_x med(r^i )^2 $$By applying Equation (9), LMSE has very high BP (up to 0·5) but poor efficiency, which makes it a proper function for outlier diagnosis (Rousseeuw, Reference Rousseeuw1984). At this time, we could adjust the weight wi of each measurement yi. Before fixing the weights, the threshold M is calculated by:
$$i_0 = \min \{ i:r^{(i)} \geqslant \eta \}, \; \; \; \; q = \mathop {\min} \limits_{i \geqslant i_0} \left( {\displaystyle{{i - 1} \over {G(r^{(i)} )}}} \right)$$
$$M = \lfloor q \rfloor $$where 
$\left\lfloor \cdot \right\rfloor $ denotes the integer part.
The strategy of adjusting weight wi is:
$$w^i = \left\{ {\matrix{ 0 \cr 1 \cr} \; \; \; \; \; \; \; \; \matrix{ {\displaystyle{{|r^i |} \over {\sigma _e}} \gt 2\; {\rm and}\; \; r^{(i)} \gt i_0 \;} \cr {{\rm else}} \cr}} \right.$$Thus the recursive procedure of robust correlation matching is summarised as:
1. Utilise x 0 to form the residuals
$r_0^i = y^i - h(\hat x_0 )^i, i = 1, \cdots, N,$ and define the weights $\bar w_0^i $ based on Equation (12);2. Update the estimation
$\hat x_{\,j + 1} $ by $\hat x_{\,j + 1} = \arg \mathop {\min} \limits_x \left\{ {\mathop \sum \limits_{i = 1}^N [w_j^i (y_t^i - h({\rm x})^i )]^2} \right\};$3. Update weights according Equation (12);
4. Iterate until convergence.
5. DEALING WITH MULTIMODALITY IN THE LMS MATCHING RESULT
Obviously, in the robust correlation matching, the influence of the outliers are expected to be approximately eliminated by firstly employing the LMS matching. After that, the weight of each observation is adjusted recursively to improve the performance of the correlation matching. However, a problem could be raised by the similarity of the bathymetric terrain. More specifically, the similarity of the terrain may lead to a multimodal phenomenon in the result of the LMS matching, which means the second highest peak instead of the first highest in the PDF may be the closest to the true value. In fact, when the terrain is relatively flat, the multimodality is inevitable. When this happens, the iteration based on the highest peak leads to an error.
To address this problem, instead of updating the weights according the residuals of the highest peak in the LMS matching result, we adjust the weights according to each peak respectively in the first iteration, and trace every set of weights independently in the subsequent iterations. Thus, at the end of the iterations, rather than one set of weights, it will result in several different sets of weights, each set corresponding to a peak in the result of the LMS matching. With these m sets of weights {w ji, i=1,…, m; j=1,…, N}, the final weights {w jF, j=1,…, N} could be fixed by applying the Bayesian equation.
$$w_j^F = \mathop \sum \limits_{i = 1}^m p(i) \cdot w_j^i = \mathop \sum \limits_{i = 1}^m \alpha ^i \cdot p_i (y_t |x_t )$$In Equation (13), p(i) is the probability of the ith peak of the LMS matching result being the true position of x t, and αi is the normalised parameter associated with p(i). By appling LSE principle, αi is given as:
$${{\alpha ^i = \left( {\displaystyle{1 \over {R^i}}} \right)^2} \left/ {\mathop \sum \limits_{i = 1}^m \left( {\displaystyle{1 \over {R^i}}} \right)^2}} $$where Ri is the residual value:
$$R^i = \mathop \sum \limits_{\,j = 1}^N \left[w_j^i (y_t^j - h(x_t )^j )\right]^2 $$The process of the robust matching algorithm can be illustrated by the flowchart in Figure 1. Notice that since the strategy of weight adjustment is only 0 or 1 in every step of iteration, weight adjustment corresponding to different peaks could lead to the same results, because these peaks have similar terrain information. When this happens, the tracing of these peaks can be merged, and the time consumed for the tracing could be reduced significantly.
The process of robust correlation matching algorithm.
6. FILTERING ALGORITHM
By the robust matching method, outliers in the terrain data are removed. Thus, a filtering method can be used for recursive estimation. In the model equations, the process Equation (1) is linear while the measurement Equation (2) is nonlinear. There are several filtering methods that can work with this case, such as PF and the mass point filter. In this paper, the Gauss sum filter is utilised for recursively positioning, because its efficiency is relatively higher in low-dimension cases (Runnalls et al., Reference Runnalls, Groves and Handley2005). By representing the posterior PDF of position x with a finite set of Gaussian distribution, the measurement update step is executed by using Bayes's theorem.
$$p(x_{t + 1} |y_{t + 1} ) = L(y_{t + 1} {\rm |}x_{t + 1} )p(x_t |y_t )$$where L(y t+1|x t+1) is the likelihood function of y t+1 with respect to x t+1. Given the weights {w jF, j=1,…, N} acquired by the robust matching, L(y t|x t) is calculated as:
$$L(y_t {\rm |}x_t ) = \displaystyle{1 \over {\sqrt {(2\pi \sigma _e^2 )^N}}} {\rm exp}\left( { - \displaystyle{1 \over {2\sigma _e^2}} \mathop \sum \limits_{\,j = 1}^N \; w_j^F (y_t^j - h^j (x_t ))^2} \right)$$More details on Gauss sum filter can be found in Anderson and Moore (Reference Anderson and Moore2012) and Runnalls et al. (Reference Runnalls, Groves and Handley2005).
7. EXPERIMENTS AND ANALYSIS
In order to test the proposed method mentioned above, experiments with simulated data and real terrain map data were conducted.
In the simulations, the vehicle is assumed to be equipped with a multi-beam sonar system with 121 beams to collect real-time terrain data during the navigation. The background terrain information is provided by a priori seafloor maps, acquired by a multi-beam bathymetric system in the South China Sea. The vehicle is supposed to travel in a long track from A to B at 5 knots constant velocity, as shown in Figure 2, The distance between neighbouring measurement epochs is set to be 50 m.
The track of the vehicle and underwater background map.
In the simulation, the sonar is assumed to have a measurement error of 1% (RMS) of the measured distance, and the noise level of the background-map depth error is set to be 0·2 m (RMS). Meanwhile, the INS is assumed to have a drift rate of 1% of the horizontal distance travelled. In the terrain correlation matching, batch processing is performed with five consecutive swaths of terrain measurements, and a search area of ±50 metres in each direction is used.
We first investigated the performance of the robust matching algorithm when there were no outliers in terrain data. Figure 3 shows typical matching errors of the ordinary method and the robust matching method. The ordinary method is based on the principle described in Equation (11), while the robust matching method is based on the algorithm described in Section 4. As can be seen, both methods yield estimates with accuracy within 10 metres. Obviously, when there are no outliers in measurements, the robust matching method provides a similar result to the ordinary method.
Comparison of results between different methods when no outliers exist.
Secondly, we investigated the performance of the robust matching method when outliers occurred in real-time terrain measurements. In the simulation, a contamination of 5% is artificially added to measurements, where the amplitude of outliers is set at 1 m. With the different type of measurements, the matching results are presented in Figure 4. It is obvious that the ordinary matching method is badly disturbed, the positioning errors grew to 20–30 metres. The robust matching method evidently performs better by providing a steady positioning result. This result strongly advocates the use of the robust matching algorithm in UTRN.
Comparison of results between two methods with outliers in terrain measurements.
When we change the type of outlier by adding gross errors in the terrain map, similar results show that the ordinary matching method is sensitive to outliers in the map. Figure 5 presents matching results using a terrain map with 5% outliers randomly added in the searching area. It can be seen that the ordinary method is still disturbed by the outliers, but in a reduced degree when compared to Figure 4. There are two reasons for this phenomenon. Firstly, the randomness of the outlier distribution decreases the probability of outliers occurring in the data h(x), which is the background terrain data corresponding to the true position. Secondly, for outliers occurring in h(x), the matching algorithm will recognise the position 
$\tilde x$ as the true position when the background terrain data of 
$\tilde x$ is not only without outliers but also has similar measurements. Such position 
$\tilde x$ is often placed in the neighbourhood of the true position x. As a result, the bias in the matching result is relatively smaller. For the robust matching, the horizontal error of the matching result is not sensitive to outliers in the terrain map, as is shown in Figure 5.
Comparison of results with outliers in terrain model.
To compare the influence of outliers on the matching method, the matching results in different situations are displayed in Table 1. This shows that the matching results of robust matching are close to the ordinary method when there are no outliers. The outlier's influence is obvious in the ordinary method but it is well reduced in the proposed robust matching algorithm. The average time consumption of the two matching methods are also provided. It is clear that the robust matching method on average consumes more time than the ordinary method. This is foreseeable given that the robust matching algorithm has to trace all the peaks in the PDF of the LMS matching result. In addition, it can be seen that there is not much difference in the average time consumption between the matching results with or without outliers. This coincides with the fact that the LMS matching is very robust against outliers.
Positioning results of different situations.
Finally, an investigation into the performance of the robust matching algorithm in the high contamination case, simulated with a higher fraction of contamination, was carried out. The results are shown in Figure 6, which are mean errors from 20 Monte Carlo runs. It can be seen from this figure that the positioning error increases generally with the contamination rate. However, the robust matching algorithm can provide a reasonable positioning result with up to 40% of the contamination rate, which proves the robustness of the proposed matching method.
Horizontal error of the robust matching algorithm when the fraction of contamination increases.
8. CONCLUDING REMARKS
Outliers in the terrain data worsen the UTRN solution. To address this problem, a robust matching algorithm is proposed in this paper. By applying this method, the contamination in terrain data can be handled. Experimental results have demonstrated that the proposed method can obtain more accurate and more robust navigation solutions in the presence of outliers.
