EKF SLAM should be finished now.

Still some lines of pseudocode instead of actual code. This should only be lines which obtain data from Roel/Gabi though... meaning that I can translate those into actual code after tomorrow