C++ implementation of Extended Kalman Filter