working on SLAM2D unit tests