More reasonable covariance, fixed encoder rotational measurements, and fixed encoder overflow bug