Created an EkfSLAM class implementing the generic SLAM interface. Added lots of TODO comments in locations where I saw possibility for optimization.

Only adapted the algorithm-related code to the new interface, did not yet change the test code to conform to the new interface, so test code is broken.

IssueID #10