Work on Slam

- parameter tuning in the example
- debug output