Research Article  Open Access
Dongdong Peng, Tian Zhou, Chao Xu, Wanyuan Zhang, Jiajun Shen, "Marginalized Point Mass Filter with Estimating Tidal Depth Bias for Underwater TerrainAided Navigation", Journal of Sensors, vol. 2019, Article ID 7340130, 15 pages, 2019. https://doi.org/10.1155/2019/7340130
Marginalized Point Mass Filter with Estimating Tidal Depth Bias for Underwater TerrainAided Navigation
Abstract
Terrainaided 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 twodimensional point mass filter, only estimating the horizontal position, has been proven to be effective for terrainaided navigation. However, the tidal depth bias is unpredictable or predicts in many cases, which will result in the rapid performance degradation if a twodimensional 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 twodimensional 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 longterm 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 singlebeam altimeter, it is natural to use the measured terrain from the payload sensors to assist with navigation. This specific type of navigation, known as terrainaided 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 2dimensional (2D) state space model only estimating the horizontal position, due to the excessively computational complexity with the higher state dimension. A realistic problem with the 2D 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 2D 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 2D PMF is still used. To address this, a 2D 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 3dimensional PMF (3D PMF) using threedimensional grids is introduced by Anonsen and Hallingstad [16]. However, this method needs to calculate a threedimensional convolution, which dramatically increases the computational demands and is not feasible in realtime 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 3D but estimated by the Kalman filter. Compared with the 3D PMF using 3D grids and the 2D 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 3D PMF since it avoids a threedimensional 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 singlebeam sonar which is not available for the 2D with the relative profile method.
Motivated by the RaoBlackwellized or marginalized PF [20–22], the concept of marginalized or RaoBlackwellized 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 RaoBlackwellized 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, 2D 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 TerrainAided 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 northeastdown 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 righthand 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 2dimensional 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 firstorder Markov process. Thus, the 3dimensional 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 terrainaided navigation, such as a multibeam sonar (MBS), singlebeam sonar, and DVL. For these sensors, two types of measurement models, namely, projectionbased and rangebased, can be adopted [10]. Compared to a rangebased measurement model which requires computationally intensive ray tracing, a projectionbased measurement model allows quick access to a digital map for matching calculations. As a result, a projectionbased measurement model is adopted here.
Assuming that a MBS is used, as shown in Figure 1, the projectionbased 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 threedimensional 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 .
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 2D. 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 onestep predictive probability density calculated by ChapmanKolmogorov 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. TwoDimensional Point Mass Filter
The tidal depth bias can be compensated if it is predicted in advance, which will lead to a 2D 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 2D PMF can be implemented [16].
In the 2D 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 2D 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 2D convolution, which is a high computational demand in the 2D PMF. A matrix operation is implemented to reduce the demand by dividing the 2D convolution into a twodimensional 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 2D 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 2D PMF are referred in [8]. Here, a 2D PMF algorithm procedure for TAN is outlined in Algorithm 1.

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 3D PMF, as described by Anonsen and Hallingstad [16]. However, the 3D PMF is impractical for realtime TAN due to its high computational complexity with 3D 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 RaoBlackwellized 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 2D 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, onestep 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 RaoBlackwellized 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 2D 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 2D 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 2D 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 indexbased strategy is introduced, which is similar to the resampling in PF [28]. A graphical interpretation of an indexbased 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 indexbased adaptive grid design is presented in Algorithm 2. Different from the 2D 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)

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 shipbased 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.
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)
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 2D 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.

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 2D PMF converge to a stable estimate after 50 seconds, with typical RMS errors around 5 meters. Compared with the 2D 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)
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 2D PMF in both cases. The performance of 2D 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)
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 2D 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 2D 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.

(a)
(b)
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 2D PMF diverges as the maximum of terminal horizontal position error is around 205 m. In addition, the mean and variance of distribution for 2D PMF increase dramatically as the tidal depth increases. This suggests that the 2D 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)
(a)
(b)
(c)
(d)
(a)
(b)

It should be noted that the 2D 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^{2}; 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)
Then, with different measurement noise covariances m^{2}, 3 m^{2}, and 5 m^{2}, 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)
(a)
(b)
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 terrainaided navigation. Simulation experiments with the multibeam sonar have been conducted in different types of terrain. With no added tidal depth bias, both the 2D PMF and marginalized PMF perform well with the high accuracy of around 510 m in two different types of terrain. When the tidal depth bias exists, the 2D 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 timevarying 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 Startup Grant Program of Postdoctoral Researchers settled in Heilongjiang (Grant number LBHQ18042).
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. View at: Publisher Site  Google Scholar
 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. View at: Google Scholar
 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. View at: Publisher Site  Google Scholar
 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. View at: Publisher Site  Google Scholar
 W. Kedong and Y. Yang, “Influence of application conditions on terrainaided navigation,” in 2010 8th World Congress on Intelligent Control and Automation, pp. 391–396, Jinan, China, 2010. View at: Publisher Site  Google Scholar
 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. View at: Publisher Site  Google Scholar
 I. Nygren, Terrain navigation for underwater vehicles, [Ph.D. thesis], Royal Institute of Technology (KTH), Stockholm, Sweden, 2005.
 N. Bergman, Recursive Bayesian estimationnavigation and tracking applications, [Ph.D. thesis], Linköping University, Linköping, Sweden, 1999.
 M. Šimandl, J. Královec, and T. Söderström, “Advanced pointmass method for nonlinear state estimation,” Automatica, vol. 42, no. 7, pp. 1133–1145, 2006. View at: Publisher Site  Google Scholar
 D. K. Meduna, Terrain relative navigation for sensorlimited 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. View at: Publisher Site  Google Scholar
 B. Claus and R. Bachmayer, “Terrainaided navigation for an underwater glider,” Journal of Field Robotics, vol. 32, no. 7, pp. 935–951, 2015. View at: Publisher Site  Google Scholar
 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. View at: Publisher Site  Google Scholar
 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. View at: Publisher Site  Google Scholar
 F. C. Teixeira, J. Quintas, P. Maurya, and A. Pascoal, “Robust particle filter formulations with application to terrainaided navigation,” International Journal of Adaptive Control and Signal Processing, vol. 31, no. 4, pp. 608–651, 2017. View at: Publisher Site  Google Scholar
 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. View at: Publisher Site  Google Scholar
 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. View at: Publisher Site  Google Scholar
 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 terrainreferenced navigation,” IEEE Transactions on Aerospace and Electronic Systems, vol. 51, no. 4, pp. 3461–3469, 2015. View at: Publisher Site  Google Scholar
 P. J. Nordlund and F. Gustafsson, “Marginalized particle filter for accurate and reliable terrainaided navigation,” IEEE Transactions on Aerospace and Electronic Systems, vol. 45, no. 4, pp. 1385–1399, 2009. View at: Publisher Site  Google Scholar
 T. Schon, F. Gustafsson, and P. J. Nordlund, “Marginalized particle filters for mixed linear/nonlinear statespace models,” IEEE Transactions on Signal Processing, vol. 53, no. 7, pp. 2279–2289, 2005. View at: Publisher Site  Google Scholar
 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. View at: Publisher Site  Google Scholar
 V. Šmídl and M. Gašperin, “RaoBlackwellized point mass filter for reliable state estimation,” in Proceedings of the 16th International Conference on Information Fusion, pp. 312–318, Istanbul, Turkey, 2013. View at: Google Scholar
 C. J. Bordin and M. G. S. Bruno, “Distributed RaoBlackwellized point mass filter for blind equalization in receiver networks,” in 2015 23rd European Signal Processing Conference (EUSIPCO), pp. 2186–2190, Nice, France, 2015. View at: Publisher Site  Google Scholar
 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. View at: Google Scholar
 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. View at: Google Scholar
 J. Dunik, M. Sotak, M. Vesely, O. Straka, and W. Hawkinson, “Design of RaoBlackwellised pointmass filter with application in terrain aided navigation,” IEEE Transactions on Aerospace and Electronic Systems, vol. 55, no. 1, pp. 251–272, 2018. View at: Publisher Site  Google Scholar
 F. Gustafsson, “Particle filter theory and practice with positioning applications,” IEEE Aerospace and Electronic Systems Magazine, vol. 25, no. 7, pp. 53–82, 2010. View at: Publisher Site  Google Scholar
Copyright
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.