EKF SLAM appears to be working :O

that is, first hardcoded test runs without errors and results seem decent at first glance