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

Terrain-aided navigation is a promising approach to submerged position updates for autonomous underwater vehicles by matching terrainmeasurements against an underwater referencemap.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.


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][13][14][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][21][22], the concept of marginalized or Rao-Blackwellized PMF is generally discussed in [23], and its two versions are applied in blind equalization in receiver networks [24] and vehicle speed tracking [25], respectively.In particular, the filter is also applied in TAN [26,27].In these papers, a fixed grid design for these Rao-Blackwellized PMFs is used during the entire filtering process.The advantage of fixed grid design is that it is easy to implement the Kalman filter update.However, in TAN, the grid support expands in the process prediction step.The number of grid points will increase as the grid support expands if the resolution is fixed during the entire filtering process, which will lead to a gradual increase in computation.On the contrary, the resolution will degrade as the grid support expands if the number of grid points is fixed which will sacrifice the approximate accuracy of probability density.
Different from the fixed grid design method, an adaptive grid design method for the marginalized PMF is adopted in this paper to ensure the effectiveness of computation and accuracy during the entire filtering process.In the method, the grid is inserted or removed, and the indexes corresponding to these grid points are saved.Based on the indexes, the Kalman filter state and covariance corresponding to each inserted or removed grid point are added or removed.It is similar to the resampling in PF [28] and facilitates the implementation of the Kalman filter update for each grid point.The feasibility of the proposed method is demonstrated by the simulation experiments.
The remainder of the paper is organized as follows: In Section 2, underwater TAN models including a state space model and a measurement model are introduced, followed by the recursive Bayesian estimation theory, 2-D PMF, and marginalized PMF in Section 3. Section 4 gives the simulation experiment results and analysis with a real underwater reference map and simulation setup, before some conclusions are given in Section 5.

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.

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 northeast-down frame is used as a navigation coordinate system {n}, where the origin is chosen in the centre of a vehicle.A vehicle body coordinate system {b} is a Cartesian frame where x 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 where x k is the estimated state vector, u k,k−1 is the control input, and w k 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 However, the tidal depth bias needs to be added to the state vector and estimated when it is unknown.Due to the fact that the tidal depth bias is usually constant or slowly varying during the mission, it can be modeled as a first-order Markov process.Thus, the 3-dimensional state, control input, and noise vectors are given by where δz k is the tidal depth bias at time k.Now, w k 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 Q k .
2.2.Measurement Model.Any sensor that is capable of perceiving the underwater terrain can be used for terrain-aided navigation, such as a multibeam sonar (MBS), single-beam sonar, and DVL.For these sensors, two types of measurement models, namely, projection-based and range-based, can be adopted [10].Compared to a range-based measurement model which requires computationally intensive ray tracing, a projection-based measurement model allows quick access to a digital map for matching calculations.As a result, a projection-based measurement model is adopted here.
Assuming that a MBS is used, as shown in Figure 1, the projection-based measurement model for the range measurements of N b beams in one ping is given by where y k 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 z k from a depth pressure sensor, as follows: where I is a N b -dimensional vector that has all elements 1 and q k is a matrix with three rows and N b 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 a i , across track distance c i , and altitude d i , as follows: where r i is a range measurement of the ith beam for a given beam azimuth α i and along trajectory angle β.
The rotation transformation matrix from the vehicle body frame {b} to the navigation frame {n}, represented by the symbol C n b , is related to the attitude of the vehicle.It can be calculated by where γ, θ, and ψ represent the roll, pitch, and heading angles, respectively.H is a constant matrix, given by The symbol h ⋅ 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: in the neighborhood of x N , x E , λ is a set of weights, and N m is the number of neighboring grid points.
As described in state space model, δz k is the tidal depth bias, which is as a known quantity in the case of 2-D.v k is the measurement noise, consisting of range measurement errors, depth pressure sensor errors, interpolation errors, and map creation errors.For simplicity, v k is modeled as a Gaussian white noise with mean zero and covariance R k .

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 p x k | y 1 k be a posterior probability density of the estimated state, conditioned on a history of measurements y 1 k .According to the Bayesian formula, it can be calculated by where p y k | x k is the likelihood function, related to the measurement model, p x k | y 1 k−1 is the one-step predictive probability density calculated by Chapman-Kolmogorov equation (7), and p x k | x k−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 Given the initial probability density of the state, the posterior probability density at each time k 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.

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., δz k = 0.A 2-D PMF can be implemented [16].
In the 2-D PMF, the probability density p x k | y 1 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 x k i, j , where i = 1 … M and j = 1 … N. Finally, the probability density p x k | y 1 k 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 p w k ⋅ and p v k ⋅ represent the probability density for process noise and measurement noise, respectively.η k denotes a normalized factor, given by The estimated position and covariance matrix are then calculated by Journal of Sensors 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 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 2-D PMF since many of the grid points whose weights are close to zero can be neglected in time and measurement updates.In the adaptive grid method, the grid is refined by inserting new grid points if the number of effective grid points N eff gets below a minimum number N 0 , whereas the grid is decimated by removing every other row and column when N eff exceeds a maximum number N 1 .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.

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.

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 p x k | y 1 k can be decomposed as where p δz k | x n k , y 1 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 p x n k | y 1 k is intractable and still estimated using the original 2-D PMF.
If x n k i, j has been approximated by a set of grid points with weights, the conditional probability density of tidal depth bias at a grid point x n k i, j can be given by where N ⋅ denotes a Gaussian probability density and δ z k i, j and P δz k i, j denote the conditional mean and covariance, respectively, which are calculated by the Kalman filter, as follows: where equation (15)

Journal of Sensors
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 p v k ′ ⋅ denotes a new Gaussian probability density, given by The estimated position and its covariance are still computed by equation (12).The estimated tidal depth bias and its covariance are computed by 3.3.2.Design of Grid and Adaption.In marginalized or Rao-Blackwellized PMF for TAN, a fixed grid design is usually used during the entire filtering process.For example, the grid resolution is fixed in [26], while the number of grid points is fixed in [27].This fixed grid design method is different from that in 2-D PMF in Section 3.2 and facilitates the implementation of the Kalman filter update.However, in TAN, the grid support expands in time update step.If the resolution is fixed during the entire filtering process, the number of grid points will increase as the grid support expands, which will lead to a gradual increase in computation.On the contrary, the resolution will degrade as the grid support expands if the number of grid points is fixed which will sacrifice the approximate accuracy of probability density.To ensure the effectiveness of computation and accuracy during the entire filtering process, an adaptive grid design method which is the same as that in 2-D PMF is still needed.
In the adaptive grid method, the grid is refined by inserting new grid points if the number of effective grid points N eff gets below a minimum number N 0 , whereas the grid is decimated by removing every other row and column when N eff exceeds a maximum number N 1 , 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 Old grid point with index New grid point with index

Simulation Experiments
In this section, simulation experiments are carried out in a real underwater reference map to assess the TAN performance of the proposed algorithm.Due to the limitation of practical condition, the test data sets from the sensors which include the INS, multibeam sonar, and depth pressure sensor are obtained through computer simulation.
4.1.Underwater Digital Reference Map.The underwater reference map, represented by a digital elevation model (DEM), is constructed from a survey data set collected with a ship-based MBS, designed and operated by Harbin Engineering University in a lake.Firstly, the survey data is postprocessed by outlier elimination and smooth filtering and formed into a XYZ file with longitude, latitude, and depth.
To simplify, we convert the positions for longitude and latitude to Universal Transverse Mercator (UTM) positions.
Then, these positions are transformed into the map coordinate system with origin starting at zero by translation and rotation.Finally, the data is gridded with 5 m horizontal resolution.As shown in Figure 3, the size of the reference map is about 1 km × 2 5 km, and the water depth varies from 10 m to 70 m.7 Journal of Sensors 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 R k .The attitude from the INS and the depth from the depth pressure sensor are assumed to be accurate.
The parameters of filter settings are listed in Table 1.In addition to the tidal depth bias parameter, other parameters of horizontal position in both the marginalized PMF and 2-D PMF are the same.The initial tidal depth bias is assumed to be a Gaussian distribution with mean 0. The minimum number N 0 and maximum number N 1 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.

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 δz k = 0 m, δz k = 1 m, and δz k = 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.

Results in Area
A with Different Tidal Biases.Firstly, with no added tidal depth bias (δz k = 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).
Then, with the added tidal depth biases of 1 m and 2 m (δz k = 1 m and δz k = 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        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.Finally, to verify the stable positioning performance, the statistical distributions of terminal horizontal position error for 50 MC runs from two filters are examined.And the mean and minimum and maximum of terminal horizontal position error are also shown in Table 2.The terminal horizontal position error is referred to as the difference between true position and estimated position at the last moment.As shown in Figure 7 and Table 2, the mean and variance of terminal position error distribution from 2-D PMF increase slowly as the tidal depth bias increase, whereas the marginalized PMF maintains a stable distribution.Moreover, it can be also seen that the maximum of terminal horizontal position error is 87.81 m in the 2-D PMF with 1 m tidal depth bias, which means that the occasional divergence occurred.The results indicate that the marginalized PMF is able to recover the correct position even if the tidal depth bias exists.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.
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 (δz k = 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.

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., δz k = 1 m.
Firstly, let measurement noise covariance R k = 1 m 2 ; 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 are shown in   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.

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

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

Figure 2 :
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.

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 (Algorithm 2 :
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 Figure4(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.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 (1) Initialization: initialize grid points x n 0 i, j with weights p x n 0 i, j | y −1 and Kalman filter δz 0|−1 i, j and covariance P δz 0|−1 i, j (2) Point mass filter measurement update: calculate the normalized posterior density p x n k i, j | y 1 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 δz k i, j and covariance P δz k i, j according to equation (15) (5) Calculate the tidal depth bias estimate and covariance according to equation (18) (6) Index-based adaptive grid: if N eff > N 1 , remove grid points if N eff < N 0 , insert new grid points (7) Point mass filter time update: propagate the predictive probability density p x n k i, j | y 1 k−1 according to equation (10) (8) Kalman filter time update: calculate the prediction δz k|k−1 i, j and covariance P δz k|k−1 i, j according to equation (16) (9) Return to step 2 Marginalized point mass filter in three dimensions.

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

Figure 4 :
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.

Figure 5 :Figure 6 :
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 δz k = 0 m and (b) tidal depth bias estimate with δz k = 0 m.

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

Figure 8 :
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 δz k = 0 m and (b) tidal depth bias estimate with δz k = 0 m.

4. 3 . 2 .Figure 9 :
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 δz k = 1 m; (b) tidal depth bias estimate with δz k = 1 m; (c) horizontal position error with δz k = 2 m; (d) tidal depth bias estimate with δz k = 2 m.

Figure 11 .Figure 10 :
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).

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

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

Figure 13 :
Figure 13: Horizontal position and tidal depth bias estimate RMS errors with different measurement noise covariances (R k = 1 m 2 , 3 m 2 , and 5 m 2 ) from the marginalized PMF for 50 Monte Carlo runs with δz k = 1 m in area B: (a) horizontal position estimate RMS error and (b) tidal depth bias estimate RMS error.
(16)esents the Kalman filter measurement update; equation(16)represents the Kalman filter time update; G k i, j , δz k|k−1 i, j , and P δz k|k−1 i, j are the Kalman gain, one-step prediction, and covariance, respectively; and Q δz k is the tidal depth bias noise.

Table 1 :
Parameters of filter settings.

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.

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.