Robust and Efficient Smoothing for Underwater Navigation