Some tuning to the Rolling window navigation, started to setup SLAM