bug fixes in ekf: prevents crashes if first datapoint is from vision instead of
IMUs