A self-consistent bathymetric mapping algorithm