The paper has been accepted at the Robotics: Science and Systems (RSS 2025). Latest version can be found on arXiv.