Change navigation algorithm and bug fix