Update hw4_nav.m

fixed backtracking and priority queue is now looking for closest to goal instead of shortest path cost doesn't seem to have changed behavior