Debugging of nonlinear EKF underway