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