Paper accepted in RA-L and IROS 2021! :tada: