Refactor the navigation algorithm

Add a description of Refactor the navigation algorithm