Earlier update

2021-03-25

Reviewed for IEEE Robotics and Automation Letters (RA-L) & IROS 2021