Extended Kalman filter (non-linear) implemented and tested.