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

Algorithm 2

Marginalized point mass filter in three dimensions.

(1) Initialization:

    initialize grid points with weights

     and Kalman filter and

    covariance

(2) Point mass filter measurement update:

    calculate the normalized posterior density

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

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

(4) Kalman filter measurement update:

    calculate the conditional mean and covariance

     according to equation (15)

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

(6) Index-based adaptive grid:

    if , remove grid points

    if , insert new grid points

(7) Point mass filter time update:

    propagate the predictive probability density

     according to equation (10)

(8) Kalman filter time update:

    calculate the prediction and covariance

     according to equation (16)

(9) Return to step 2