Development of a navigation algorithm for autonomous underwater vehicles