Added uncertainty checks for in position and correct heading.