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

Authors: Dongdong Peng; Tian Zhou; Chao Xu; Wanyuan Zhang; Jiajun Shen
Journal: Journal of Sensors
 (2019)

---

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


---

## Body

## 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 Section2, 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 wherex axis points forward, y axis points to the starboard side, and z axis obeys the right-hand rule. According to the vehicle’s motion, a state space model based on INS is given by
(1)xk=xk−1+uk,k−1+wk,where xk is the estimated state vector, uk,k−1 is the control input, and wk 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(2)xk=xk,Nxk,EΤ,uk,k−1=Δxk,NΔxk,EΤ,wk=wk,Nwk,EΤ,where xk,N and xk,E represent the vehicle’s north and east positions at time k, respectively; uk,k−1 represents the corresponding position changes, provided by the INS measurement; wk 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(3)xk=xk,Nxk,EδzkT,uk,k−1=Δxk,NΔxk,E0T,wk=wk,Nwk,Ewk,DT,where δzk is the tidal depth bias at time k. Now, wk 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 Qk.


### 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 Figure1, the projection-based measurement model for the range measurements of Nb beams in one ping is given by
(4)yk=hxk,Nxk,EΤ+H⋅Cbn⋅qk+δzk⋅Ι+vk,where yk is a vector, representing the total depth from sea bottom to the sea level. It is a combination of the altitude measurements d from the MBS and the depth measurement zk from a depth pressure sensor, as follows:
(5a)yk=zk⋅I+d,where I is a Nb-dimensional vector that has all elements 1 and qk is a matrix with three rows and Nb columns, representing three-dimensional space locations of all beam footprints in the vehicle body frame {b}. Its ith component can be decomposed by the along track distance ai, across track distance ci, and altitude di, as follows:
(5b)qk,i=aicidiT=ricosαisinβrisinαiricosαicosβT,where ri is a range measurement of the ith beam for a given beam azimuth αi 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 symbolCbn, is related to the attitude of the vehicle. It can be calculated by
(5c)Cbn=cosθcosψ−cosθsinψsinθcosγsinψ+sinγsinθcosψcosγcosψ−sinγsinθsinψ−sinγcosθsinγsinψ−cosγsinθcosψsinγcosψ+cosγsinθsinψcosγcosθ,where γ, θ, and ψ represent the roll, pitch, and heading angles, respectively. H is a constant matrix, given by
(5d)H=100010.The symbolh⋅ 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:
(5e)hxN,xE=∑i=1Nmλihmapxi,Nmap,xi,Emap,where hxN,xE denotes the estimated depth at the required interpolation point xN,xE, hmapxNmap,xEmap is a vector of map values corresponding to grid points xNmap,xEmap in the neighborhood of xN,xE, λ is a set of weights, and Nm is the number of neighboring grid points.As described in state space model,δzk is the tidal depth bias, which is as a known quantity in the case of 2-D. vk is the measurement noise, consisting of range measurement errors, depth pressure sensor errors, interpolation errors, and map creation errors. For simplicity, vk is modeled as a Gaussian white noise with mean zero and covariance Rk.


## 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 wherex axis points forward, y axis points to the starboard side, and z axis obeys the right-hand rule. According to the vehicle’s motion, a state space model based on INS is given by
(1)xk=xk−1+uk,k−1+wk,where xk is the estimated state vector, uk,k−1 is the control input, and wk 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(2)xk=xk,Nxk,EΤ,uk,k−1=Δxk,NΔxk,EΤ,wk=wk,Nwk,EΤ,where xk,N and xk,E represent the vehicle’s north and east positions at time k, respectively; uk,k−1 represents the corresponding position changes, provided by the INS measurement; wk 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(3)xk=xk,Nxk,EδzkT,uk,k−1=Δxk,NΔxk,E0T,wk=wk,Nwk,Ewk,DT,where δzk is the tidal depth bias at time k. Now, wk 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 Qk.


## 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 Figure1, the projection-based measurement model for the range measurements of Nb beams in one ping is given by
(4)yk=hxk,Nxk,EΤ+H⋅Cbn⋅qk+δzk⋅Ι+vk,where yk is a vector, representing the total depth from sea bottom to the sea level. It is a combination of the altitude measurements d from the MBS and the depth measurement zk from a depth pressure sensor, as follows:
(5a)yk=zk⋅I+d,where I is a Nb-dimensional vector that has all elements 1 and qk is a matrix with three rows and Nb columns, representing three-dimensional space locations of all beam footprints in the vehicle body frame {b}. Its ith component can be decomposed by the along track distance ai, across track distance ci, and altitude di, as follows:
(5b)qk,i=aicidiT=ricosαisinβrisinαiricosαicosβT,where ri is a range measurement of the ith beam for a given beam azimuth αi 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 symbolCbn, is related to the attitude of the vehicle. It can be calculated by
(5c)Cbn=cosθcosψ−cosθsinψsinθcosγsinψ+sinγsinθcosψcosγcosψ−sinγsinθsinψ−sinγcosθsinγsinψ−cosγsinθcosψsinγcosψ+cosγsinθsinψcosγcosθ,where γ, θ, and ψ represent the roll, pitch, and heading angles, respectively. H is a constant matrix, given by
(5d)H=100010.The symbolh⋅ 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:
(5e)hxN,xE=∑i=1Nmλihmapxi,Nmap,xi,Emap,where hxN,xE denotes the estimated depth at the required interpolation point xN,xE, hmapxNmap,xEmap is a vector of map values corresponding to grid points xNmap,xEmap in the neighborhood of xN,xE, λ is a set of weights, and Nm is the number of neighboring grid points.As described in state space model,δzk is the tidal depth bias, which is as a known quantity in the case of 2-D. vk is the measurement noise, consisting of range measurement errors, depth pressure sensor errors, interpolation errors, and map creation errors. For simplicity, vk is modeled as a Gaussian white noise with mean zero and covariance Rk.


## 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. Letpxk∣y1:k be a posterior probability density of the estimated state, conditioned on a history of measurements y1:k. According to the Bayesian formula, it can be calculated by
(6)pxk∣y1:k=pyk∣xkpxk∣y1:k−1∫pyk∣xkpxk∣y1:k−1dxk,(7)pxk∣y1:k−1=∫pxk∣xk−1pxk−1∣y1:k−1dxk−1,where pyk∣xk is the likelihood function, related to the measurement model, pxk∣y1:k−1 is the one-step predictive probability density calculated by Chapman-Kolmogorov equation (7), and pxk∣xk−1 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(8)x^k=∫xkpxk∣y1:kdxk,P^k=∫xk−x^kxk−x^kΤpxk∣y1:kdxk.Given the initial probability density of the state, the posterior probability density at each timek 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., δzk=0. A 2-D PMF can be implemented [16].In the 2-D PMF, the probability densitypxk∣y1:k 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 M and N grid points in each direction, denoted as xki,j, where i=1…M and j=1…N. Finally, the probability density pxk∣y1:k is approximated at each grid point by a weight pxki,j∣y1:k.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:
(9)pxki,j∣y1:k=ηk−1pvkyk−hxki,j+H⋅Cbn⋅qk⋅pxki,j∣y1:k−1,(10)pxki,j∣y1:k−1=∑m=1M∑n=1Npwkxki,j−uk,k−1−xk−1m,n⋅pxk−1m,n∣y1:k−1Δ2,where pwk⋅ and pvk⋅ represent the probability density for process noise and measurement noise, respectively. ηk denotes a normalized factor, given by
(11)δk=∑i=1M∑j=1Npvkyk−hxki,j−H⋅Cbn⋅qk⋅pδxki,j∣y1:k−1Δ2.The estimated position and covariance matrix are then calculated by(12)x^k=∑m=1M∑n=1Nxki,jpxki,j∣y1:kΔ2,P^k=∑i=1M∑j=1Nxki,j−x^kxki,j−x^kΤ⋅pxki,j∣y1:kΔ2.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 pointsNeff gets below a minimum number N0, whereas the grid is decimated by removing every other row and column when Neff exceeds a maximum number N1. 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.Algorithm 1: Two-dimensional point mass filter (2-D PMF).
(1) Initialization:
initialize grid pointsx0i,j with weights
px0i,j∣y−1
(2) Measurement update:
calculate the normalized posterior density
pxki,j∣y1:k according to equation (9)
(3) Calculate the position estimate and its covariance according to equation (12)
(4) Adaptive grid:
ifNeff>N1, remove grid points
ifNeff<N0, insert new grid points
(5) Time update:
propagate the predictive probability density
pxki,j∣y1:k−1 according to equation (10)
(6) Return to step 2


### 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 xkn=xk,Nxk,EΤ, the joint posterior probability density pxk∣y1:k can be decomposed as
(13)pxk∣y1:k=pδzk,xkn∣y1:k=pδzk∣xkn,y1:k⋅pxkn∣y1:k,where pδzk∣xkn,y1:k denotes probability density of the tidal depth bias conditioned on the horizontal position. It is analytically tractable and estimated using the Kalman filter, while pxkn∣y1:k is intractable and still estimated using the original 2-D PMF.Ifxkni,j has been approximated by a set of grid points with weights, the conditional probability density of tidal depth bias at a grid point xkni,j can be given by
(14)pδzk∣xkni,j,y1:k=Nδzki,j,Pδzki,j,where N⋅ denotes a Gaussian probability density and δzki,j and Pδzki,j denote the conditional mean and covariance, respectively, which are calculated by the Kalman filter, as follows:
(15)δzki,j=δzk∣k−1i,j+Gki,jyk−hxkni,j+H⋅Cbn⋅qk,Pδzki,j=Pδzk∣k−1i,j−Gki,jIPδzk∣k−1i,j,Gki,j=Pδzk∣k−1i,jIΤIPδzk∣k−1i,jIΤ+Rk−1,(16)δzk∣k−1i,j=δzk−1i,j,Pδzk∣k−1i,j=Pδzk−1i,j+Qδzk,where equation (15) represents the Kalman filter measurement update; equation (16) represents the Kalman filter time update; Gki,j, δzk∣k−1i,j, and Pδzk∣k−1i,j are the Kalman gain, one-step prediction, and covariance, respectively; and Qδzk 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
(17a)pxkni,j∣y1:k=ηk−1pvk′yk−hxkni,j+H⋅Cbn⋅qk−δzk∣k−1i,j⋅I⋅pxki,j∣y1:k−1,where pvk′⋅ denotes a new Gaussian probability density, given by
(17b)pvk′⋅~N0,IPδzkk−1i,jIΤ+Rk.The estimated position and its covariance are still computed by equation (12). The estimated tidal depth bias and its covariance are computed by
(18)δz^k=∑i=1M∑j=1Npxkni,jδzki,jΔ2,P^δzk=∑i=1M∑j=1Npxkni,jPδzki,j+δzki,j−δz^kδzki,j−δz^kΤΔ2.


#### 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 pointsNeff gets below a minimum number N0, whereas the grid is decimated by removing every other row and column when Neff exceeds a maximum number N1, 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).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.
(a)
(b)Algorithm 2: Marginalized point mass filter in three dimensions.
(1) Initialization:
initialize grid pointsx0ni,j with weights
px0ni,j∣y−1 and Kalman filter δz0∣−1i,j and
covariancePδz0∣−1i,j
(2) Point mass filter measurement update:
calculate the normalized posterior density
pxkni,j∣y1:k 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δzki,j and covariance
Pδzki,j according to equation (15)
(5) Calculate the tidal depth bias estimate and covariance according to equation (18)
(6) Index-based adaptive grid:
ifNeff>N1, remove grid points
ifNeff<N0, insert new grid points
(7) Point mass filter time update:
propagate the predictive probability density
pxkni,j∣y1:k−1 according to equation (10)
(8) Kalman filter time update:
calculate the predictionδzk∣k−1i,j and covariance
Pδzk∣k−1i,j according to equation (16)
(9) Return to step 2


## 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. Letpxk∣y1:k be a posterior probability density of the estimated state, conditioned on a history of measurements y1:k. According to the Bayesian formula, it can be calculated by
(6)pxk∣y1:k=pyk∣xkpxk∣y1:k−1∫pyk∣xkpxk∣y1:k−1dxk,(7)pxk∣y1:k−1=∫pxk∣xk−1pxk−1∣y1:k−1dxk−1,where pyk∣xk is the likelihood function, related to the measurement model, pxk∣y1:k−1 is the one-step predictive probability density calculated by Chapman-Kolmogorov equation (7), and pxk∣xk−1 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(8)x^k=∫xkpxk∣y1:kdxk,P^k=∫xk−x^kxk−x^kΤpxk∣y1:kdxk.Given the initial probability density of the state, the posterior probability density at each timek 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., δzk=0. A 2-D PMF can be implemented [16].In the 2-D PMF, the probability densitypxk∣y1:k 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 M and N grid points in each direction, denoted as xki,j, where i=1…M and j=1…N. Finally, the probability density pxk∣y1:k is approximated at each grid point by a weight pxki,j∣y1:k.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:
(9)pxki,j∣y1:k=ηk−1pvkyk−hxki,j+H⋅Cbn⋅qk⋅pxki,j∣y1:k−1,(10)pxki,j∣y1:k−1=∑m=1M∑n=1Npwkxki,j−uk,k−1−xk−1m,n⋅pxk−1m,n∣y1:k−1Δ2,where pwk⋅ and pvk⋅ represent the probability density for process noise and measurement noise, respectively. ηk denotes a normalized factor, given by
(11)δk=∑i=1M∑j=1Npvkyk−hxki,j−H⋅Cbn⋅qk⋅pδxki,j∣y1:k−1Δ2.The estimated position and covariance matrix are then calculated by(12)x^k=∑m=1M∑n=1Nxki,jpxki,j∣y1:kΔ2,P^k=∑i=1M∑j=1Nxki,j−x^kxki,j−x^kΤ⋅pxki,j∣y1:kΔ2.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 pointsNeff gets below a minimum number N0, whereas the grid is decimated by removing every other row and column when Neff exceeds a maximum number N1. 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.Algorithm 1: Two-dimensional point mass filter (2-D PMF).
(1) Initialization:
initialize grid pointsx0i,j with weights
px0i,j∣y−1
(2) Measurement update:
calculate the normalized posterior density
pxki,j∣y1:k according to equation (9)
(3) Calculate the position estimate and its covariance according to equation (12)
(4) Adaptive grid:
ifNeff>N1, remove grid points
ifNeff<N0, insert new grid points
(5) Time update:
propagate the predictive probability density
pxki,j∣y1:k−1 according to equation (10)
(6) Return to step 2


## 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 xkn=xk,Nxk,EΤ, the joint posterior probability density pxk∣y1:k can be decomposed as
(13)pxk∣y1:k=pδzk,xkn∣y1:k=pδzk∣xkn,y1:k⋅pxkn∣y1:k,where pδzk∣xkn,y1:k denotes probability density of the tidal depth bias conditioned on the horizontal position. It is analytically tractable and estimated using the Kalman filter, while pxkn∣y1:k is intractable and still estimated using the original 2-D PMF.Ifxkni,j has been approximated by a set of grid points with weights, the conditional probability density of tidal depth bias at a grid point xkni,j can be given by
(14)pδzk∣xkni,j,y1:k=Nδzki,j,Pδzki,j,where N⋅ denotes a Gaussian probability density and δzki,j and Pδzki,j denote the conditional mean and covariance, respectively, which are calculated by the Kalman filter, as follows:
(15)δzki,j=δzk∣k−1i,j+Gki,jyk−hxkni,j+H⋅Cbn⋅qk,Pδzki,j=Pδzk∣k−1i,j−Gki,jIPδzk∣k−1i,j,Gki,j=Pδzk∣k−1i,jIΤIPδzk∣k−1i,jIΤ+Rk−1,(16)δzk∣k−1i,j=δzk−1i,j,Pδzk∣k−1i,j=Pδzk−1i,j+Qδzk,where equation (15) represents the Kalman filter measurement update; equation (16) represents the Kalman filter time update; Gki,j, δzk∣k−1i,j, and Pδzk∣k−1i,j are the Kalman gain, one-step prediction, and covariance, respectively; and Qδzk 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
(17a)pxkni,j∣y1:k=ηk−1pvk′yk−hxkni,j+H⋅Cbn⋅qk−δzk∣k−1i,j⋅I⋅pxki,j∣y1:k−1,where pvk′⋅ denotes a new Gaussian probability density, given by
(17b)pvk′⋅~N0,IPδzkk−1i,jIΤ+Rk.The estimated position and its covariance are still computed by equation (12). The estimated tidal depth bias and its covariance are computed by
(18)δz^k=∑i=1M∑j=1Npxkni,jδzki,jΔ2,P^δzk=∑i=1M∑j=1Npxkni,jPδzki,j+δzki,j−δz^kδzki,j−δz^kΤΔ2.


### 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 pointsNeff gets below a minimum number N0, whereas the grid is decimated by removing every other row and column when Neff exceeds a maximum number N1, 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).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.
(a)
(b)Algorithm 2: Marginalized point mass filter in three dimensions.
(1) Initialization:
initialize grid pointsx0ni,j with weights
px0ni,j∣y−1 and Kalman filter δz0∣−1i,j and
covariancePδz0∣−1i,j
(2) Point mass filter measurement update:
calculate the normalized posterior density
pxkni,j∣y1:k 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δzki,j and covariance
Pδzki,j according to equation (15)
(5) Calculate the tidal depth bias estimate and covariance according to equation (18)
(6) Index-based adaptive grid:
ifNeff>N1, remove grid points
ifNeff<N0, insert new grid points
(7) Point mass filter time update:
propagate the predictive probability density
pxkni,j∣y1:k−1 according to equation (10)
(8) Kalman filter time update:
calculate the predictionδzk∣k−1i,j and covariance
Pδzk∣k−1i,j according to equation (16)
(9) Return to step 2


## 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 xkn=xk,Nxk,EΤ, the joint posterior probability density pxk∣y1:k can be decomposed as
(13)pxk∣y1:k=pδzk,xkn∣y1:k=pδzk∣xkn,y1:k⋅pxkn∣y1:k,where pδzk∣xkn,y1:k denotes probability density of the tidal depth bias conditioned on the horizontal position. It is analytically tractable and estimated using the Kalman filter, while pxkn∣y1:k is intractable and still estimated using the original 2-D PMF.Ifxkni,j has been approximated by a set of grid points with weights, the conditional probability density of tidal depth bias at a grid point xkni,j can be given by
(14)pδzk∣xkni,j,y1:k=Nδzki,j,Pδzki,j,where N⋅ denotes a Gaussian probability density and δzki,j and Pδzki,j denote the conditional mean and covariance, respectively, which are calculated by the Kalman filter, as follows:
(15)δzki,j=δzk∣k−1i,j+Gki,jyk−hxkni,j+H⋅Cbn⋅qk,Pδzki,j=Pδzk∣k−1i,j−Gki,jIPδzk∣k−1i,j,Gki,j=Pδzk∣k−1i,jIΤIPδzk∣k−1i,jIΤ+Rk−1,(16)δzk∣k−1i,j=δzk−1i,j,Pδzk∣k−1i,j=Pδzk−1i,j+Qδzk,where equation (15) represents the Kalman filter measurement update; equation (16) represents the Kalman filter time update; Gki,j, δzk∣k−1i,j, and Pδzk∣k−1i,j are the Kalman gain, one-step prediction, and covariance, respectively; and Qδzk 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
(17a)pxkni,j∣y1:k=ηk−1pvk′yk−hxkni,j+H⋅Cbn⋅qk−δzk∣k−1i,j⋅I⋅pxki,j∣y1:k−1,where pvk′⋅ denotes a new Gaussian probability density, given by
(17b)pvk′⋅~N0,IPδzkk−1i,jIΤ+Rk.The estimated position and its covariance are still computed by equation (12). The estimated tidal depth bias and its covariance are computed by
(18)δz^k=∑i=1M∑j=1Npxkni,jδzki,jΔ2,P^δzk=∑i=1M∑j=1Npxkni,jPδzki,j+δzki,j−δz^kδzki,j−δz^kΤΔ2.


## 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 pointsNeff gets below a minimum number N0, whereas the grid is decimated by removing every other row and column when Neff exceeds a maximum number N1, 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).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.
(a)
(b)Algorithm 2: Marginalized point mass filter in three dimensions.
(1) Initialization:
initialize grid pointsx0ni,j with weights
px0ni,j∣y−1 and Kalman filter δz0∣−1i,j and
covariancePδz0∣−1i,j
(2) Point mass filter measurement update:
calculate the normalized posterior density
pxkni,j∣y1:k 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δzki,j and covariance
Pδzki,j according to equation (15)
(5) Calculate the tidal depth bias estimate and covariance according to equation (18)
(6) Index-based adaptive grid:
ifNeff>N1, remove grid points
ifNeff<N0, insert new grid points
(7) Point mass filter time update:
propagate the predictive probability density
pxkni,j∣y1:k−1 according to equation (10)
(8) Kalman filter time update:
calculate the predictionδzk∣k−1i,j and covariance
Pδzk∣k−1i,j according to equation (16)
(9) Return to step 2


## 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 Figure3, the size of the reference map is about 1km×2.5km, 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. Figure4(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.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.
(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.,β=0. 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 0 and covariance Rk. 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 Table1. 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 N0 and maximum number N1 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
32 m2
Position process noise covariance
diag(22, 22) m2
Measurement noise covariance
12I m2
Depth bias noise variance
0.052 m2
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δzk=0 m, δzk=1 m, and δzk=2 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 (δzk=0 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).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δzk=0 m and (b) tidal depth bias estimate with δzk=0 m.
(a)
(b)Then, with the added tidal depth biases of 1 m and 2 m (δzk=1 m and δzk=2 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.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δzk=1 m; (b) tidal depth bias estimate with δzk=1 m; (c) horizontal position error with δzk=2 m; (d) tidal depth bias estimate with δzk=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 Table2. 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]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).
(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 Figures8–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.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δzk=0 m and (b) tidal depth bias estimate with δzk=0 m.
(a)
(b)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δzk=1 m; (b) tidal depth bias estimate with δzk=1 m; (c) horizontal position error with δzk=2 m; (d) tidal depth bias estimate with δzk=2 m.
(a)
(b)
(c)
(d)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).
(a)
(b)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 (δzk=1 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.,δzk=1 m.Firstly, let measurement noise covarianceRk=1 m2; the estimated north, east, and tidal errors with the respective 3 standard deviations (3σ) 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.Figure 11
Estimate errors of north, east, and tide with respective 3 standard deviations (3σ) from marginalized PMF for 50 Monte Carlo runs with δzk=1 m and Rk=1 m2 in different terrain areas A and B: estimate errors with 3 standard deviations (3σ) in (a) area A and (b) area B.
(a)
(b)Then, with different measurement noise covariancesRk=1 m2, 3 m2, and 5 m2, 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.Figure 12
Horizontal position and tidal depth bias estimate RMS errors with different measurement noise covariances (Rk=1 m2, 3 m2, and 5 m2) from the marginalized PMF for 50 Monte Carlo runs with δzk=1 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 (Rk=1 m2, 3 m2, and 5 m2) from the marginalized PMF for 50 Monte Carlo runs with δzk=1 m in area B: (a) horizontal position estimate RMS error and (b) tidal depth bias estimate RMS error.
(a)
(b)


## 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 Figure3, the size of the reference map is about 1km×2.5km, 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. Figure4(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.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.
(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.,β=0. 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 0 and covariance Rk. 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 Table1. 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 N0 and maximum number N1 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
32 m2
Position process noise covariance
diag(22, 22) m2
Measurement noise covariance
12I m2
Depth bias noise variance
0.052 m2
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δzk=0 m, δzk=1 m, and δzk=2 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 (δzk=0 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).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δzk=0 m and (b) tidal depth bias estimate with δzk=0 m.
(a)
(b)Then, with the added tidal depth biases of 1 m and 2 m (δzk=1 m and δzk=2 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.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δzk=1 m; (b) tidal depth bias estimate with δzk=1 m; (c) horizontal position error with δzk=2 m; (d) tidal depth bias estimate with δzk=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 Table2. 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]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).
(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 Figures8–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.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δzk=0 m and (b) tidal depth bias estimate with δzk=0 m.
(a)
(b)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δzk=1 m; (b) tidal depth bias estimate with δzk=1 m; (c) horizontal position error with δzk=2 m; (d) tidal depth bias estimate with δzk=2 m.
(a)
(b)
(c)
(d)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).
(a)
(b)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 (δzk=1 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.,δzk=1 m.Firstly, let measurement noise covarianceRk=1 m2; the estimated north, east, and tidal errors with the respective 3 standard deviations (3σ) 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.Figure 11
Estimate errors of north, east, and tide with respective 3 standard deviations (3σ) from marginalized PMF for 50 Monte Carlo runs with δzk=1 m and Rk=1 m2 in different terrain areas A and B: estimate errors with 3 standard deviations (3σ) in (a) area A and (b) area B.
(a)
(b)Then, with different measurement noise covariancesRk=1 m2, 3 m2, and 5 m2, 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.Figure 12
Horizontal position and tidal depth bias estimate RMS errors with different measurement noise covariances (Rk=1 m2, 3 m2, and 5 m2) from the marginalized PMF for 50 Monte Carlo runs with δzk=1 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 (Rk=1 m2, 3 m2, and 5 m2) from the marginalized PMF for 50 Monte Carlo runs with δzk=1 m in area B: (a) horizontal position estimate RMS error and (b) tidal depth bias estimate RMS error.
(a)
(b)


## 4.3.1. Results in Area A with Different Tidal Biases

Firstly, with no added tidal depth bias (δzk=0 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).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δzk=0 m and (b) tidal depth bias estimate with δzk=0 m.
(a)
(b)Then, with the added tidal depth biases of 1 m and 2 m (δzk=1 m and δzk=2 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.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δzk=1 m; (b) tidal depth bias estimate with δzk=1 m; (c) horizontal position error with δzk=2 m; (d) tidal depth bias estimate with δzk=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 Table2. 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]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).
(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 Figures8–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.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δzk=0 m and (b) tidal depth bias estimate with δzk=0 m.
(a)
(b)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δzk=1 m; (b) tidal depth bias estimate with δzk=1 m; (c) horizontal position error with δzk=2 m; (d) tidal depth bias estimate with δzk=2 m.
(a)
(b)
(c)
(d)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).
(a)
(b)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 (δzk=1 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.,δzk=1 m.Firstly, let measurement noise covarianceRk=1 m2; the estimated north, east, and tidal errors with the respective 3 standard deviations (3σ) 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.Figure 11
Estimate errors of north, east, and tide with respective 3 standard deviations (3σ) from marginalized PMF for 50 Monte Carlo runs with δzk=1 m and Rk=1 m2 in different terrain areas A and B: estimate errors with 3 standard deviations (3σ) in (a) area A and (b) area B.
(a)
(b)Then, with different measurement noise covariancesRk=1 m2, 3 m2, and 5 m2, 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.Figure 12
Horizontal position and tidal depth bias estimate RMS errors with different measurement noise covariances (Rk=1 m2, 3 m2, and 5 m2) from the marginalized PMF for 50 Monte Carlo runs with δzk=1 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 (Rk=1 m2, 3 m2, and 5 m2) from the marginalized PMF for 50 Monte Carlo runs with δzk=1 m in area B: (a) horizontal position estimate RMS error and (b) tidal depth bias estimate RMS error.
(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 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.



---

*Source: 7340130-2019-05-02.xml*