Robust and Efficient Terrain Navigation of

For terrain navigation to be a serious navigation tool in underwater navigation it must be robust and work well in flat bottomed areas. Furthermore it should be easy to incorporate with the vehicle's inertial navigation system (INS) so a bound can be placed on the system's position error. This paper describes the terrain navigation system developed for the Swedish Defence Materiel Administration's autonomous vehicles AUV62F and Sapphires. Both vehicles are battery powered and torpedo shaped with a diameter of 21". From the outset, the terrain navigation system was designed to work in flat bottomed areas; it uses many simultaneous sonar beams (400+) to measure the bottom topography, producing a unique underwater map position for the vehicle. When terrain navigating in flat bottomed areas, bottom topography measurement often gives many possible vehicle positions, i.e., the probability density function of the vehicle position is multimodal, so efficient and robust nonlinear Kalman filtering must be used. The terrain navigation module uses an optimal nonlinear Kalman filter called the FD filter. The FD filter numerically solves the stochastic differential equation that guides the vehicle positioning. The measurement updating is Bayesian. The filtering procedure is characterized by robustness, simplicity, and accuracy. It is also simple to incorporate independent measurements other than the terrain topography into the filter.