Optimal estimation is a promising tool for multi-contact inertial estimation and localization. To harness its advantages in robotics, it is crucial to solve these large and challenging optimization problems efficiently. To tackle this, we (i) develop a multiple-shooting solver that exploits both temporal and parametric structures through a parametrized Riccati recursion. Additionally, we (ii) propose an inertial local manifold that ensures its full physical consistency. It also enhances convergence compared to the singularity-free log-Cholesky approach. To handle its singularities, we (iii) introduce a nullspace approach in our optimal estimation solver. We (iv) finally develop the analytical derivatives of contact dynamics for both inertial parametrizations. Our framework can successfully solve estimation problems for complex maneuvers such as brachiation in humanoids. We demonstrate its numerical capabilities across various robotics tasks and its benefits in experimental trials with the Go1 robot.

翻译：最优估计是多接触惯性估计与定位领域极具前景的工具。为充分发挥其在机器人领域的优势，关键需高效求解这些规模庞大且具有挑战性的优化问题。为此，我们（i）开发了一种多点打靶求解器，通过参数化Riccati递推同时利用时间与参数结构。此外，（ii）提出一种惯性局部流形以保证其完全物理一致性——相较于无奇异性的对数-乔列斯基方法，该方法还能增强收敛性。为处理其奇异性，（iii）在最优估计求解器中引入零空间方法。（iv）最终推导了两种惯性参数化下接触动力学的解析导数。该框架可成功解算人形机器人摆荡等复杂动作的估计问题，通过多种机器人任务的数值实验及Go1机器人实物验证，展示了其计算性能与实用性优势。