Extended Kalman Filter implementation (not completed)