Update 2d slam with new crsm_slam