First working code with the algorithms I plan to submit to a journal
Code of the paper submitted to IROS 2017