Added more logic for updating attitude and position