Journal of Sensors
Volume 2019 (2019), Article ID 7340130, 15 pages
https://doi.org/10.1155/2019/7340130

Research Article

Marginalized Point Mass Filter with Estimating Tidal Depth Bias for Underwater Terrain-Aided Navigation

Dongdong Peng,1,2,3 Tian Zhou,1,2,3 Chao Xu,1,2,3 Wanyuan Zhang,1,2,3 and Jiajun Shen1,2,3

¹Acoustic Science and Technology Laboratory, Harbin Engineering University, Harbin 150001, China
²Key Laboratory of Marine Information Acquisition and Security (Harbin Engineering University), Ministry of Industry and Information Technology, Harbin 150001, China
³College of Underwater Acoustic Engineering, Harbin Engineering University, Harbin 150001, China

Correspondence should be addressed to Chao Xu; xuchao18@hrbeu.edu.cn

Received 30 June 2018; Revised 1 January 2019; Accepted 17 January 2019; Published 2 May 2019

Academic Editor: Giovanni Diraco

Copyright © 2019 Dongdong Peng et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract.

Terrain-aided navigation is a promising approach to submerged position updates for autonomous underwater vehicles by matching terrain measurements against an underwater reference map. With an accurate prediction of tidal depth bias, a two-dimensional point mass filter, only estimating the horizontal position, has been proven to be effective for terrain-aided navigation. However, the tidal depth bias is unpredictable or predicts in many cases, which will result in the rapid performance degradation if a two-dimensional point mass filter is still used. To address this, a marginalized point mass filter in three dimensions is presented to concurrently estimate and compensate the tidal depth bias in this paper. In the method, the tidal depth bias is extended as a state variable and estimated using the Kalman filter, whereas the horizontal position state is still estimated by the original two-dimensional point mass filter. With the multibeam sonar, simulation experiments in a real underwater digital map demonstrate that the proposed method is able to accurately estimate the tidal depth bias and to obtain the robust navigation solution in suitable terrain.

1. Introduction

Autonomous underwater vehicles (AUVs) are increasingly applied in various types of underwater missions such as environment assessment, seabed mapping, and mine reconnaissance. Precise navigation is crucial in successfully completing these tasks [1]. So far, the inertial navigation system (INS) is a primary navigation system for most AUVs [2]. Such navigation system, however, suffers from drift error with time. In order to allow long-term submerged operations, additional underwater position fixes are necessary.

Due to the unavailable Global Positioning System (GPS) underwater, a typical approach to limit the error drift is in combination with velocity measurement from a Doppler Velocity Logs (DVL) [3]. Unfortunately, even with velocity aiding, position error of the integrated system still grows with time. Another option is to utilize underwater acoustic positioning systems like long baseline, short baseline, or ultrashort baseline. However, such systems require external infrastructure like underwater transponders or a support vessel, which will limit operational range and sacrifice the autonomy of AUVs. Since an AUV is usually equipped with a bathymetric sensor such as the multibeam sonar, DVL, or single-beam altimeter, it is natural to use the measured terrain from the payload sensors to assist with navigation. This specific type of navigation, known as terrain-aided navigation (TAN), makes an underwater vehicle true “autonomous” without external infrastructure.

So far, existing TAN methods may be grouped into the batch correlation method like TERCOM [4], ICCP [5], and maximum likelihood estimation [6, 7] and recursive Bayesian method like SITAN, point mass filter [8, 9], and particle filter [10]. Compared with the batch correlation method, the recursive Bayesian method is more sophisticated since it incorporates motion uncertainty between adjacent measurements which is not accounted for in the batch correlation method [11]. In particular, the point mass filter (PMF) and particle filter (PF), as the approximate numerical solution to the recursive Bayesian method, have become dominant for TAN in recent years [12–15]. According to Anonsen and Hallingstad [16], the PMF shows more accurate and reliable positioning performance than the PF for TAN.

This paper focuses on the PMF for TAN. In general, the PMF is implemented in a low 2-dimensional (2-D) state space model only estimating the horizontal position, due to the excessively computational complexity with the higher state dimension. A realistic problem with the 2-D state space model is that uncertain depth measurement may arise due to the tidal change between the time of current mission and the map creation [17]. If the tidal depth bias is accurately known in advance, the 2-D PMF for TAN can work well in suitable terrain. However, the tidal depth bias is unpredictable or mispredicted in some real scenes. This will result in the rapid performance degradation if a 2-D PMF is still used. To address this, a 2-D PMF with relative profiles is adopted by Nygren and Anonsen [7, 18], but this method has a limit in the use of the sensor type. Moreover, it cannot give the estimate of depth bias. To concurrently estimate and compensate the depth bias, a 3-dimensional PMF (3-D PMF) using three-dimensional grids is introduced by Anonsen and Hallingstad [16]. However, this method needs to calculate a three-dimensional convolution, which dramatically increases the computational demands and is not feasible in real-time navigation [19].

In this paper, a marginalized PMF in 3 dimensions is presented to concurrently estimate and compensate the depth bias for TAN. In the method, the tidal depth bias is also extended to 3-D but estimated by the Kalman filter. Compared with the 3-D PMF using 3-D grids and the 2-D PMF using the relative profile method, the proposed method has its own unique advantages. On one hand, the proposed method is less computational demanding than 3-D PMF since it avoids a three-dimensional convolution. On the other hand, it is able to give the tidal depth estimate without a sensor limit. It can even work in the use of single-beam sonar which is not available for the 2-D with the relative profile method.

Motivated by the Rao-Blackwellized or marginalized PF [20–22], the concept of marginalized or Rao-Blackwellized PMF is generally discussed in [23], and its two versions are applied in blind equalization in receiver networks [24] and vehicle speed tracking [25], respectively. In particular, the filter is also applied in TAN [26, 27]. In these papers, a fixed grid design for these Rao-Blackwellized PMFs is used during the entire filtering process. The advantage of fixed grid design is that it is easy to implement the Kalman filter update. However, in TAN, the grid support expands in the process prediction step. The number of grid points will increase as the grid support expands if the resolution is fixed during the entire filtering process, which will lead to a gradual increase in computation. On the contrary, the resolution will degrade as the grid support expands if the number of grid points is fixed which will sacrifice the approximate accuracy of probability density.

Different from the fixed grid design method, an adaptive grid design method for the marginalized PMF is adopted in this paper to ensure the effectiveness of computation and accuracy during the entire filtering process. In the method, the grid is inserted or removed, and the indexes corresponding to these grid points are saved. Based on the indexes, the Kalman filter state and covariance corresponding to each inserted or removed grid point are added or removed. It is similar to the resampling in PF [28] and facilitates the implementation of the Kalman filter update for each grid point. The feasibility of the proposed method is demonstrated by the simulation experiments.

The remainder of the paper is organized as follows: In Section 2, underwater TAN models including a state space model and a measurement model are introduced, followed by the recursive Bayesian estimation theory, 2-D PMF, and marginalized PMF in Section 3. Section 4 gives the simulation experiment results and analysis with a real underwater reference map and simulation setup, before some conclusions are given in Section 5.

2. Underwater Terrain-Aided Navigation Models

In general, the concept of navigation is to estimate the position, velocity, and attitude of a vehicle. Since the INS attitude update is independent of position estimate and maintains a high precision after alignment, the main purpose of TAN is to estimate a position of a vehicle here. It is completed by an estimation filter which fuses a state space model with a measurement model. In this section, these two models are described in detail.

2.1. State Space Model

The position of a vehicle is usually estimated in a global coordinate system. Here, we consider a map frame {m} as the global coordinate system. A north-east-down frame is used as a navigation coordinate system {n}, where the origin is chosen in the centre of a vehicle. A vehicle body coordinate system {b} is a Cartesian frame where axis points forward, axis points to the starboard side, and axis obeys the right-hand rule. According to the vehicle’s motion, a state space model based on INS is given by

where is the estimated state vector, is the control input, and is the process noise.

In the case of the known tidal depth bias, only estimating the horizontal position, the 2-dimensional state, control input, and noise vectors can be represented by

where and represent the vehicle’s north and east positions at time , respectively; represents the corresponding position changes, provided by the INS measurement; represents the INS position drift error in north and east directions.

However, the tidal depth bias needs to be added to the state vector and estimated when it is unknown. Due to the fact that the tidal depth bias is usually constant or slowly varying during the mission, it can be modeled as a first-order Markov process. Thus, the 3-dimensional state, control input, and noise vectors are given by

where is the tidal depth bias at time . Now, consists of the INS position drift error and a random tidal depth bias error. For simplicity, the process noise is assumed as a Gaussian white noise with mean 0 and covariance .

2.2. Measurement Model

Any sensor that is capable of perceiving the underwater terrain can be used for terrain-aided navigation, such as a multibeam sonar (MBS), single-beam sonar, and DVL. For these sensors, two types of measurement models, namely, projection-based and range-based, can be adopted [10]. Compared to a range-based measurement model which requires computationally intensive ray tracing, a projection-based measurement model allows quick access to a digital map for matching calculations. As a result, a projection-based measurement model is adopted here.

Assuming that a MBS is used, as shown in Figure 1, the projection-based measurement model for the range measurements of beams in one ping is given by

where is a vector, representing the total depth from sea bottom to the sea level. It is a combination of the altitude measurements from the MBS and the depth measurement from a depth pressure sensor, as follows:

where is a -dimensional vector that has all elements 1 and is a matrix with three rows and columns, representing three-dimensional space locations of all beam footprints in the vehicle body frame {b}. Its th component can be decomposed by the along track distance , across track distance , and altitude , as follows:

where is a range measurement of the th beam for a given beam azimuth and along trajectory angle .

Figure 1: Schematic diagram of the measurement model based on a multibeam sonar for TAN.

The rotation transformation matrix from the vehicle body frame {b} to the navigation frame {n}, represented by the symbol , is related to the attitude of the vehicle. It can be calculated by

where , , and represent the roll, pitch, and heading angles, respectively. is a constant matrix, given by

The symbol denotes a terrain interpolation function, which is used to estimate the depth at any horizontal position in the map. Here, a linear interpolation method is implemented, as follows:

where denotes the estimated depth at the required interpolation point , is a vector of map values corresponding to grid points in the neighborhood of , is a set of weights, and is the number of neighboring grid points.

As described in state space model, is the tidal depth bias, which is as a known quantity in the case of 2-D. is the measurement noise, consisting of range measurement errors, depth pressure sensor errors, interpolation errors, and map creation errors. For simplicity, is modeled as a Gaussian white noise with mean zero and covariance .

3. Recursive Bayesian Estimation Method

3.1. Bayesian Update Equations

Given a set of underwater terrain measurements, TAN estimates the vehicle’s horizontal position (including tidal depth bias if necessary), which is a state estimation problem in essence. The nonlinearity of the described measurement model motivates the use of a full Bayesian estimation method. Let be a posterior probability density of the estimated state, conditioned on a history of measurements . According to the Bayesian formula, it can be calculated by

where is the likelihood function, related to the measurement model, is the one-step predictive probability density calculated by Chapman-Kolmogorov equation (7), and is the predictive probability density taken from the state space model [27].

According to the minimum mean square error (MMSE) criterion, the estimated state and covariance matrix can be calculated by

Given the initial probability density of the state, the posterior probability density at each time can be computed recursively by equations (6) and (7). In the case where the state space model and the measurement model are linear with the Gaussian noise, the analytic solution to the equations can be obtained by the Kalman filter. However, it does not exist in the TAN problem which is a highly nonlinear estimation problem due to the nonlinear nature terrain. Therefore, some numerical approaches are usually used, such as the PMF and the PF. Here, the PMF is used to calculate these equations.

3.2. Two-Dimensional Point Mass Filter

The tidal depth bias can be compensated if it is predicted in advance, which will lead to a 2-D TAN estimation problem. In this situation, the state, control input, and process noise vectors are defined in equation (2). The tidal depth bias is supposed to be zero, i.e., . A 2-D PMF can be implemented [16].

In the 2-D PMF, the probability density is represented by a set of grid points with weights. Firstly, a search area should be chosen by the uncertainty in the initial position from the INS. Then, the chosen 2-D search area is discretized into a grid with and grid points in each direction, denoted as , where and . Finally, the probability density is approximated at each grid point by a weight .

If a square grid with the same resolution in both directions is considered, the state prediction equation (6) and the measurement update equation (7) are performed in a direct way, as follows:

where and represent the probability density for process noise and measurement noise, respectively. denotes a normalized factor, given by

The estimated position and covariance matrix are then calculated by

Notice that equation (10) can be viewed as a 2-D convolution, which is a high computational demand in the 2-D PMF. A matrix operation is implemented to reduce the demand by dividing the 2-D convolution into a two-dimensional convolution under the assumption that the process noise is uncorrelated and equal in grid directions.

Furthermore, an adaptive grid method is also used to improve the efficiency of the 2-D PMF since many of the grid points whose weights are close to zero can be neglected in time and measurement updates. In the adaptive grid method, the grid is refined by inserting new grid points if the number of effective grid points gets below a minimum number , whereas the grid is decimated by removing every other row and column when exceeds a maximum number . An effective grid point is a point where the weight is more than times the average weight. More details about the implementation of the 2-D PMF are referred in [8]. Here, a 2-D PMF algorithm procedure for TAN is outlined in Algorithm 1.

(1) Initialization:

       initialize grid points with weights

(2) Measurement update:

       calculate the normalized posterior density

        according to equation (9)

(3) Calculate the position estimate and its covariance according to equation (12)

(4) Adaptive grid:

       if , remove grid points

       if , insert new grid points

(5) Time update:

       propagate the predictive probability density

        according to equation (10)

(6) Return to step 2

Algorithm 1: Two-dimensional point mass filter (2-D PMF).

3.3. Marginalized Point Mass Filter in Three Dimensions

The tidal depth bias needs to be estimated and compensated if it is unpredictable. By extending it as a state variable, the state, control input, and process noise vectors are defined as equation (3). In this situation, it is natural to implement a 3-D PMF, as described by Anonsen and Hallingstad [16]. However, the 3-D PMF is impractical for real-time TAN due to its high computational complexity with 3-D grids. Considering that the tidal depth bias has linear substructure in both state space and measurement models, a marginalized PMF in three dimensions is proposed to reduce the computational complexity. This approach is similar to the marginalized PF or Rao-Blackwellized PMF.

3.3.1. Marginalized Point Mass Filter

The idea of the marginalized PMF is to marginalize out the linear variable from the state space and measurement models and to estimate it by an optimal filter. For a state vector in equation (3), the tidal depth bias variable is linear, and the horizontal position variable is nonlinear. Let , the joint posterior probability density can be decomposed as

where denotes probability density of the tidal depth bias conditioned on the horizontal position. It is analytically tractable and estimated using the Kalman filter, while is intractable and still estimated using the original 2-D PMF.

If has been approximated by a set of grid points with weights, the conditional probability density of tidal depth bias at a grid point can be given by

where denotes a Gaussian probability density and and denote the conditional mean and covariance, respectively, which are calculated by the Kalman filter, as follows:

where equation (15) represents the Kalman filter measurement update; equation (16) represents the Kalman filter time update; , , and are the Kalman gain, one-step prediction, and covariance, respectively; and is the tidal depth bias noise.

Since the measurement model is now associated with the tidal depth bias, the calculation of position likelihood probability should involve the tidal depth bias. Thus, the measurement update equation (9) becomes

where denotes a new Gaussian probability density, given by

The estimated position and its covariance are still computed by equation (12). The estimated tidal depth bias and its covariance are computed by

3.3.2. Design of Grid and Adaption

In marginalized or Rao-Blackwellized PMF for TAN, a fixed grid design is usually used during the entire filtering process. For example, the grid resolution is fixed in [26], while the number of grid points is fixed in [27]. This fixed grid design method is different from that in 2-D PMF in Section 3.2 and facilitates the implementation of the Kalman filter update. However, in TAN, the grid support expands in time update step. If the resolution is fixed during the entire filtering process, the number of grid points will increase as the grid support expands, which will lead to a gradual increase in computation. On the contrary, the resolution will degrade as the grid support expands if the number of grid points is fixed which will sacrifice the approximate accuracy of probability density. To ensure the effectiveness of computation and accuracy during the entire filtering process, an adaptive grid design method which is the same as that in 2-D PMF is still needed.

In the adaptive grid method, the grid is refined by inserting new grid points if the number of effective grid points gets below a minimum number , whereas the grid is decimated by removing every other row and column when exceeds a maximum number , as described in 2-D PMF. However, in marginalized PMF, each grid corresponds to a Kalman filter with the state and covariance. It should be noticed that the Kalman filter state and covariance need to change as the grid changes in the adaptive grid step 6. For example, if a grid point is removed in adaptive grid step 6, the Kalman filter state and covariance corresponding to the removed grid point are removed. Instead, the Kalman filter state and covariance corresponding to the new grid point are inserted if a new grid point is inserted.

To facilitate the implementation of inserting or removing for the Kalman filter update, an index-based strategy is introduced, which is similar to the resampling in PF [28]. A graphical interpretation of an index-based adaptive grid in marginalized PMF is shown in Figure 2. In the step of inserting, a new grid point is believed to be sampled from the grid point closest to it, and its index is the same as that grid point and saved, as described in Figure 2(a). In the step of removing, the index of a new grid point is obtained by removing every other row and column, as described in Figure 2(b). Based on these indexes, the Kalman filter state and covariance corresponding to the inserted or removed grid points are copied or removed. The marginalized PMF with index-based adaptive grid design is presented in Algorithm 2. Different from the 2-D PMF, the adaptive grid is inserted and removed by the respective index, and the position measurement update is modified in equations (17a) and (17b).

(a)

(b)

Figure 2: Illustration of inserting and removing grid points in an index-based adaptive grid method: (a) inserting and (b) removing grid points with indexes.

(1) Initialization:

       initialize grid points with weights

        and Kalman filter and

       covariance

(2) Point mass filter measurement update:

       calculate the normalized posterior density

        according to equations (17a) and (17b)

(3) Calculate the position estimate and covariance according to equation (12)

(4) Kalman filter measurement update:

       calculate the conditional mean and covariance

        according to equation (15)

(5) Calculate the tidal depth bias estimate and covariance according to equation (18)

(6) Index-based adaptive grid:

       if , remove grid points

       if , insert new grid points

(7) Point mass filter time update:

       propagate the predictive probability density

        according to equation (10)

(8) Kalman filter time update:

       calculate the prediction and covariance

        according to equation (16)

(9) Return to step 2

Algorithm 2: Marginalized point mass filter in three dimensions.

4. Simulation Experiments

In this section, simulation experiments are carried out in a real underwater reference map to assess the TAN performance of the proposed algorithm. Due to the limitation of practical condition, the test data sets from the sensors which include the INS, multibeam sonar, and depth pressure sensor are obtained through computer simulation.

4.1. Underwater Digital Reference Map

The underwater reference map, represented by a digital elevation model (DEM), is constructed from a survey data set collected with a ship-based MBS, designed and operated by Harbin Engineering University in a lake. Firstly, the survey data is postprocessed by outlier elimination and smooth filtering and formed into a XYZ file with longitude, latitude, and depth. To simplify, we convert the positions for longitude and latitude to Universal Transverse Mercator (UTM) positions. Then, these positions are transformed into the map coordinate system with origin starting at zero by translation and rotation. Finally, the data is gridded with 5 m horizontal resolution. As shown in Figure 3, the size of the reference map is about , and the water depth varies from 10 m to 70 m.

Figure 3: An underwater reference map with 5 m grid resolution.

4.2. Simulation Setup

In the simulation, the vehicle is assumed to travel in a lawnmower behavior at a constant velocity 2 m/s. The total time is 600 seconds. In order to investigate the performance of the algorithms in different types of terrain, two different terrain areas are chosen, labeled as area A and area B. Figure 4(a) shows the simulated true trajectories in two terrain areas. Meanwhile, the corresponding variations of water depth along the trajectories in two terrain areas are given in Figure 4(b). It can be seen that the water depth beneath the vehicle in area A changes from 20 m to 45 m, while it changes within the range of 5 m in area B. That is to say, area A contains more variable terrain than area B, and it may be more suitable for TAN.

(a)

(b)

Figure 4: Simulated trajectories in an underwater reference map and corresponding changes of water depth along the trajectories: (a) two true trajectories in the map and (b) changes of water depth.

To simulate the INS drift error, a constant velocity error 0.1 m/s in both north and east directions is added to the true velocity. The accumulated initial position error of INS is set to be 50 m in both directions. The MBS is assumed to generate 127 beams for each ping over a 120 degree swath across the trajectory. The beam fan is vertically down, i.e., . And the measurement frequency is 1 Hz. Considering the large measurement noise of beams on outer edge and the limitation of map grid resolution, only 11 beams are chosen by sampling uniformly from each ping. The measurement noise combining MBS noise and depth pressure sensor noise is modeled as a Gaussian with mean and covariance . The attitude from the INS and the depth from the depth pressure sensor are assumed to be accurate.

The parameters of filter settings are listed in Table 1. In addition to the tidal depth bias parameter, other parameters of horizontal position in both the marginalized PMF and 2-D PMF are the same. The initial tidal depth bias is assumed to be a Gaussian distribution with mean 0. The minimum number and maximum number of grid points are 2000 and 10,000, respectively. Initial grid resolution is 5 m. Simulations are implemented in a MATLAB environment. Due to the randomness of the added measurement noise, 50 Monte Carlo runs are conducted for each filter.

Table 1: Parameters of filter settings.

Sensors and filters
Parameters

INS velocity error
[0.1; 0.1] m/s

INS sample frequency
100 Hz

MBS sample frequency
0.2 Hz

Initial position search area
300 m by 300 m

Initial depth bias variance
3² m²

Position process noise covariance
diag(2², 2²) m²

Measurement noise covariance
1²I m²

Depth bias noise variance
0.05² m²

Time step between updates
5 s

4.3. Results and Analysis

To confirm the ability of the marginalized PMF to estimate the tidal depth bias, three different tidal depth biases are added to the bathymetric measurement separately. As the tidal depth bias is relatively small in the real world, we set  m,  m, and  m. Simulations are performed in two areas A and B. The TAN performance of the filters is evaluated in terms of the root mean square (RMS) error of horizontal position and tidal depth bias estimate with 50 Monte Carlo runs.

4.3.1. Results in Area A with Different Tidal Biases

Firstly, with no added tidal depth bias ( m), two filters are tested in area A. Figure 5 show the RMS errors of horizontal position from two filters and the tidal depth bias estimate from marginalized PMF for a single run. It can be seen that both the marginalized PMF and the 2-D PMF converge to a stable estimate after 50 seconds, with typical RMS errors around 5 meters. Compared with the 2-D PMF, the marginalized PMF has slight fluctuation in the error curve. The reason is that a small tidal depth bias noise was added in the marginalized PMF. In addition, the marginalized PMF can give the estimate of tidal depth bias close to zero, as shown in Figure 5(b).

(a)

(b)

Figure 5: Horizontal position errors from two filters and tidal depth bias estimate from marginalized PMF for a single run with no tidal depth bias in area A: (a) horizontal position error with  m and (b) tidal depth bias estimate with  m.

Then, with the added tidal depth biases of 1 m and 2 m ( m and  m), two filters are tested in area A. As shown in Figure 6, the marginalized PMF is superior to the 2-D PMF in both cases. The performance of 2-D PMF gradually degrades with the increase of tidal depth bias, whereas the marginalized PMF is robust to the tidal depth bias. In addition, it can be also seen that the marginalized PMF is able to accurately estimate the tidal depth biases in the presence of the tidal depth biases of 1 m and 2 m.

(a)

(b)

(c)

(d)

Figure 6: Horizontal position errors from two filters and tidal depth bias estimate from marginalized PMF for a single run with tidal depth bias in area A: (a) horizontal position error with  m; (b) tidal depth bias estimate with  m; (c) horizontal position error with  m; (d) tidal depth bias estimate with  m.

Finally, to verify the stable positioning performance, the statistical distributions of terminal horizontal position error for 50 MC runs from two filters are examined. And the mean and minimum and maximum of terminal horizontal position error are also shown in Table 2. The terminal horizontal position error is referred to as the difference between true position and estimated position at the last moment. As shown in Figure 7 and Table 2, the mean and variance of terminal position error distribution from 2-D PMF increase slowly as the tidal depth bias increase, whereas the marginalized PMF maintains a stable distribution. Moreover, it can be also seen that the maximum of terminal horizontal position error is 87.81 m in the 2-D PMF with 1 m tidal depth bias, which means that the occasional divergence occurred. The results indicate that the marginalized PMF is able to recover the correct position even if the tidal depth bias exists.

Table 2: The mean and minimum and maximum of terminal horizontal position error for 50 Monte Carlo runs with different tidal depth biases in area A.

Tidal depth bias (m)
Methods for TAN
Mean of terminal horizontal position error (m)
Range of terminal horizontal position error [min, max] (m)

0
2-D PMF
1.63
[0.30, 6.30]

Marginalized PMF
1.81
[0.24, 5.26]

1
2-D PMF
8.39
[1.68, 87.81]

Marginalized PMF
1.73
[0.28, 6.94]

2
2-D PMF
10.39
[6.95, 15.53]

Marginalized PMF
1.62
[0.28, 6.94]

(a)

(b)

Figure 7: Horizontal position error distributions from two filters for 50 Monte Carlo runs with different tidal depth biases in area A: (a) 2-D PMF and (b) marginalized PMF (3-D).

4.3.2. Results in Area B with Different Tidal Biases

In order to study the performance of the filter in different types of terrain, simulation experiments with three different added tidal depth biases are performed in relatively flat area B. The corresponding results for two filters in area B are shown in Figures 8–10 and Table 3. It can be seen that two filters converge quickly after several measurement updates, and their final accuracy is around 5 m with no added tidal depth bias, as shown in Figure 8. This indicates that the multibeam measurements provide adequate terrain information with the approximate 100 m coverage swath across the trajectory in this area. With added tidal depth bias, the 2-D PMF diverges as the maximum of terminal horizontal position error is around 205 m. In addition, the mean and variance of distribution for 2-D PMF increase dramatically as the tidal depth increases. This suggests that the 2-D PMF easily diverges in the relatively flat area B if the tidal depth bias exists. Instead, the marginalized PMF is still robust to the tidal depth bias in this area.

(a)

(b)

Figure 8: Horizontal position errors from two filters and tidal depth bias estimate from marginalized PMF for a single run with no tidal depth bias in area B: (a) horizontal position error with  m and (b) tidal depth bias estimate with  m.

(a)

(b)

(c)

(d)

Figure 9: Horizontal position errors from two filters and tidal depth bias estimate from marginalized PMF for a single run with tidal depth bias in area B: (a) horizontal position error with  m; (b) tidal depth bias estimate with  m; (c) horizontal position error with  m; (d) tidal depth bias estimate with  m.

(a)

(b)

Figure 10: Horizontal position error distributions from two filters for 50 Monte Carlo runs with different tidal depth biases in area B: (a) 2-D PMF and (b) marginalized PMF (3-D).

Table 3: The mean and minimum and maximum of terminal horizontal position error for 50 Monte Carlo runs with different tidal depth biases in area B.

Tidal depth bias (m)
Methods for TAN
Mean of terminal horizontal position error (m)
Range of terminal horizontal position error [min, max] (m)

0
2-D PMF
5.44
[0.62, 14.36]

Marginalized PMF
5.28
[1.62, 14.64]

1
2-D PMF
115.02
[8.08, 205.40]

Marginalized PMF
5.42
[1.42, 18.96]

2
2-D PMF
224.04
[167.90, 291.68]

Marginalized PMF
5.28
[1.62, 14.64]

It should be noted that the 2-D PMF is more sensitive to the tidal depth bias in the relatively flat terrain area B, compared to the results in rough terrain area A. Even with small tidal depth bias 1 m ( m), the horizontal position RMS error reaches to 115 m, as shown in Figure 9(a). Moreover, it can be seen that the variations of horizontal position errors and tidal depth estimate over time are much smoother than that in area A. This may be caused by the interpolation error. Since the linear interpolation method is used here, there is a small interpolation error in relatively flat terrain area B and large interpolation error in rough terrain area A.

4.3.3. Comments on the Terrain Type and Measurement Noise

In terms of RMS error, the marginalized PMF for TAN using multibeam sonar performs well in the presence of different tidal depth biases as mentioned above. In order to better understand the performance of marginalized PMF in different types of terrain and different measurement noises, we examine the standard deviation provided by the marginalized PMF and investigate the influence of measurement noise in this section. Here, we set the added tidal depth bias at 1 m, i.e.,  m.

Firstly, let measurement noise covariance  m²; the estimated north, east, and tidal errors with the respective 3 standard deviations () provided for 50 Monte Carlo runs in terrain area A and B are shown in Figure 11. It can be seen that the marginalized PMF converges about 50 seconds in area A and 75 seconds in area B and provides smaller standard deviation in area A than area B. It indicates that the marginalized PMF has faster convergence and less uncertainty in rough area A than relatively flat area B.

(a)

(b)

Figure 11: Estimate errors of north, east, and tide with respective 3 standard deviations () from marginalized PMF for 50 Monte Carlo runs with  m and  m² in different terrain areas A and B: estimate errors with 3 standard deviations () in (a) area A and (b) area B.

Then, with different measurement noise covariances  m², 3 m², and 5 m², the horizontal position and tidal depth bias RMS errors in different terrain areas A and B are shown in Figures 12 and 13. It can be seen that the greater the range measurement noise is, the larger the estimate error becomes, regardless of the type of terrain. However, the filter is more sensitive to range measurement noise in relatively flat terrain area B than in rough terrain area A.

(a)

(b)

Figure 12: Horizontal position and tidal depth bias estimate RMS errors with different measurement noise covariances ( m², 3 m², and 5 m²) from the marginalized PMF for 50 Monte Carlo runs with  m in area A: (a) horizontal position estimate RMS error and (b) tidal depth bias estimate RMS error.

(a)

(b)

Figure 13: Horizontal position and tidal depth bias estimate RMS errors with different measurement noise covariances ( m², 3 m², and 5 m²) from the marginalized PMF for 50 Monte Carlo runs with  m in area B: (a) horizontal position estimate RMS error and (b) tidal depth bias estimate RMS error.

5. Conclusion

In this paper, we present a marginalized point mass filter (marginalized PMF) in 3 dimensions to concurrently estimate and compensate the tidal depth bias for underwater terrain-aided navigation. Simulation experiments with the multibeam sonar have been conducted in different types of terrain. With no added tidal depth bias, both the 2-D PMF and marginalized PMF perform well with the high accuracy of around 5-10 m in two different types of terrain. When the tidal depth bias exists, the 2-D PMF is sensitive to such depth bias, especially in the relatively flat terrain area. Instead, the marginalized PMF is able to estimate and compensate the tidal depth bias and maintain a robust navigation solution.

In future work, a more complex tidal change model needs to be established to replace the simple constant model to estimate a time-varying tidal depth bias. In addition, a more accurate INS model and a more detailed analysis of the process and measurement noise are also needed.

Data Availability

The data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest

The authors declare that there is no conflict of interest regarding the publication of this paper.

Acknowledgments

This work was supported by the National Natural Science Foundation of China (Grant number U1709203), the fund of Acoustic Science and Technology Laboratory, and the Start-up Grant Program of Postdoctoral Researchers settled in Heilongjiang (Grant number LBH-Q18042).

References

L. Paull, S. Saeedi, M. Seto, and H. Li, “AUV navigation and localization: a review,” IEEE Journal of Oceanic Engineering, vol. 39, no. 1, pp. 131–149, 2014.

K. B. Ånonsen, O. K. Hagen, Ø. Hegrenæs, and P. E. Hagen, “The HUGIN AUV terrain navigation module,” in 2013 OCEANS - San Diego, pp. 23–27, San Diego, CA, USA, September 2013.

L. Stutters, Honghai Liu, C. Tiltman, and D. J. Brown, “Navigation technologies for autonomous underwater vehicles,” IEEE Transactions on Systems, Man, and Cybernetics, Part C (Applications and Reviews), vol. 38, no. 4, pp. 581–589, 2008.

J. P. Golden, “Terrain contour matching (TERCOM): a cruise missile guidance aid,” in Image Processing for Missile Guidance, vol. 238 of Proceedings of SPIE, pp. 10–18, San Diego, CA, USA, 1980, International Society for Optics and Photonics.

W. Kedong and Y. Yang, “Influence of application conditions on terrain-aided navigation,” in 2010 8th World Congress on Intelligent Control and Automation, pp. 391–396, Jinan, China, 2010.

D. Peng, T. Zhou, H. Li, and W. Zhang, “Terrain aided navigation for underwater vehicles using maximum likelihood method,” in 2016 IEEE/OES China Ocean Acoustics (COA), pp. 1–6, Harbin, China, 2016.

I. Nygren, Terrain navigation for underwater vehicles, [Ph.D. thesis], Royal Institute of Technology (KTH), Stockholm, Sweden, 2005.

N. Bergman, Recursive Bayesian estimation-navigation and tracking applications, [Ph.D. thesis], Linköping University, Linköping, Sweden, 1999.

M. Šimandl, J. Královec, and T. Söderström, “Advanced point-mass method for nonlinear state estimation,” Automatica, vol. 42, no. 7, pp. 1133–1145, 2006.

D. K. Meduna, Terrain relative navigation for sensor-limited systems with application to underwater vehicles, [Ph.D. thesis], Stanford University, Stanford, CA, USA, 2011.

J. Melo and A. Matos, “Survey on advances on terrain based navigation for autonomous underwater vehicles,” Ocean Engineering, vol. 139, no. 1, pp. 250–264, 2017.

B. Claus and R. Bachmayer, “Terrain-aided navigation for an underwater glider,” Journal of Field Robotics, vol. 32, no. 7, pp. 935–951, 2015.

K. Zhang, Y. Li, J. Zhao, and C. Rizos, “A study of underwater terrain navigation based on the robust matching method,” Journal of Navigation, vol. 67, no. 4, pp. 569–578, 2014.

L. Zhou, X. Cheng, and Y. Zhu, “Terrain aided navigation for autonomous underwater vehicles with coarse maps,” Measurement Science and Technology, vol. 27, no. 9, article 095002, 2016.

F. C. Teixeira, J. Quintas, P. Maurya, and A. Pascoal, “Robust particle filter formulations with application to terrain-aided navigation,” International Journal of Adaptive Control and Signal Processing, vol. 31, no. 4, pp. 608–651, 2017.

K. B. Anonsen and O. Hallingstad, “Terrain aided underwater navigation using point mass and particle filters,” in 2006 IEEE/ION Position, Location, And Navigation Symposium, pp. 1027–1035, Coronado, CA, USA, 2006.

G. T. Donovan, “Position error correction for an autonomous underwater vehicle inertial navigation system (INS) using a particle filter,” IEEE Journal of Oceanic Engineering, vol. 37, no. 3, pp. 431–445, 2012.

K. B. Anonsen, Advance in terrain aided navigation for underwater vehicles, [Ph.D. thesis], Norwegian University of Science and Technology, Trondheim, Norway, 2010.

B. Copp and K. Subbarao, “Nonlinear adaptive filtering in terrain-referenced navigation,” IEEE Transactions on Aerospace and Electronic Systems, vol. 51, no. 4, pp. 3461–3469, 2015.

P. J. Nordlund and F. Gustafsson, “Marginalized particle filter for accurate and reliable terrain-aided navigation,” IEEE Transactions on Aerospace and Electronic Systems, vol. 45, no. 4, pp. 1385–1399, 2009.

T. Schon, F. Gustafsson, and P. J. Nordlund, “Marginalized particle filters for mixed linear/nonlinear state-space models,” IEEE Transactions on Signal Processing, vol. 53, no. 7, pp. 2279–2289, 2005.

T. B. Schon, R. Karlsson, and F. Gustafsson, “The marginalized particle filter in practice,” in 2006 IEEE Aerospace Conference, p. 11, Big Sky, MT, USA, 2006.

V. Šmídl and M. Gašperin, “Rao-Blackwellized point mass filter for reliable state estimation,” in Proceedings of the 16th International Conference on Information Fusion, pp. 312–318, Istanbul, Turkey, 2013.

C. J. Bordin and M. G. S. Bruno, “Distributed Rao-Blackwellized point mass filter for blind equalization in receiver networks,” in 2015 23rd European Signal Processing Conference (EUSIPCO), pp. 2186–2190, Nice, France, 2015.

M. Lindfors, G. Hendeby, F. Gustafsson, and R. Karlsson, “Vehicle speed tracking using chassis vibrations,” in 2016 IEEE Intelligent Vehicles Symposium (IV), pp. 214–219, Gothenburg, Sweden, 2016.

O. K. Hagen and K. B. Anonsen, “Improving terrain navigation by concurrent tidal and sound speed error estimation,” in 2013 OCEANS - San Diego, pp. 1–7, San Diego, CA, USA, 2013.

J. Dunik, M. Sotak, M. Vesely, O. Straka, and W. Hawkinson, “Design of Rao-Blackwellised point-mass filter with application in terrain aided navigation,” IEEE Transactions on Aerospace and Electronic Systems, vol. 55, no. 1, pp. 251–272, 2018.

F. Gustafsson, “Particle filter theory and practice with positioning applications,” IEEE Aerospace and Electronic Systems Magazine, vol. 25, no. 7, pp. 53–82, 2010.