switching to master to work on parameter
close to working slam-smooth