Added heading and speed calculations