slam-belief not working. look into alpha values. working on plotting domain in python
upsampling trajectory for plotting differs from actual points