Rewrite the EKF

Make the EKF store velocity information in the state vector and update on the fly instead of in lockstep.

CC @zbutler @kberger