Added ad-hoc solution for replanning if we change where we think we are