Got the EKF to work.