Point-mass filter and Cramer-Rao bound for terrain-aided navigation

The nonlinear estimation problem in navigation using terrain height variations is studied. The optimal Bayesian solution to the problem is derived. The implementation is grid based, calculating the probability of a set of points on an adaptively dense mesh. The Cramer-Rao bound is derived. Monte Carlo simulations over a commercial map shows that the algorithm, after convergence, reaches the Cramer-Rao lower bound.