Reset order when resetting gravity estimate
Initialize debug variables