Major EKF progress, but still completely untested.