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

Algorithm 1

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

(1) Initialization:

    initialize grid points with weights

(2) Measurement update:

    calculate the normalized posterior density

     according to equation (9)

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

(4) Adaptive grid:

    if , remove grid points

    if , insert new grid points

(5) Time update:

    propagate the predictive probability density

     according to equation (10)

(6) Return to step 2