Converted EKF to class, same look and feel as the C++ code.
Concluded C++ify, successfully tested with dataset.
Merged master / rangefinder