add robot heading to state variables