Add valid covariances. Fix incorrect direction of yaw & pitch rotation.