One paper accepted at IEEE Robotics and Automation Letters (RAL) and presented at ICRA22’, Philadelphia, USA.