Minor progress towards SLAM re-implementation