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