Add SLAM module and initial rotation/ranging