close to working slam-smooth
slam-smooth not robust. have interpolation in python working