EKF with velocity error states now localizes

There may also be something going on where the GPS is not able to back
out the entire state. I'm a little worried about that.