Simultaneous Calibration of Noise Covariance and Kinematics for State Estimation of Legged Robots via Bi-level Optimization

TL;DR

This work tackles the challenge of state estimation for legged robots in dynamic, uncertain environments by jointly calibrating process and measurement noise covariances as well as uncertain kinematics. It introduces a bi-level optimization framework where the outer level optimizes $Q$, $R$, and kinematic offsets while the inner level solves a full-information MAP estimator, with gradients obtained by differentiating through the estimator via the KKT conditions. The approach yields more accurate and consistently calibrated estimates across quadrupedal and humanoid platforms, validated on both simulation and hardware data, and demonstrates substantial improvements over hand-tuned baselines. The framework unifies estimation, sensor calibration, and kinematics identification in a principled, data-driven way that generalizes across diverse robotic platforms.

Abstract

Accurate state estimation is critical for legged and aerial robots operating in dynamic, uncertain environments. A key challenge lies in specifying process and measurement noise covariances, which are typically unknown or manually tuned. In this work, we introduce a bi-level optimization framework that jointly calibrates covariance matrices and kinematic parameters in an estimator-in-the-loop manner. The upper level treats noise covariances and model parameters as optimization variables, while the lower level executes a full-information estimator. Differentiating through the estimator allows direct optimization of trajectory-level objectives, resulting in accurate and consistent state estimates. We validate our approach on quadrupedal and humanoid robots, demonstrating significantly improved estimation accuracy and uncertainty calibration compared to hand-tuned baselines. Our method unifies state estimation, sensor, and kinematics calibration into a principled, data-driven framework applicable across diverse robotic platforms.

Figures (8)

• Figure 1: Overview of the work with its application to a quadrupedal robot.
• Figure 2: Illustration of the common sensing capabilities on legged robots: Quadrupedal Go1 and Humanoid G1 from Unitree Robotics.
• Figure 3: The robot STRIDE (a), quadrupedal robot Go1 (b), and B1 (c) are used in the evaluation (Pictures are used with permission).
• Figure 4: Calibration results on STIRDE: (a) the convergence of the upper loss function and norm of the gradients, (b) the convergence of kinematics bias w.r.t. ground truth, (c) and (d) the linear velocities of the ground truth, initial estimates, and calibrated estimates.
• Figure 5: Calibration results on Go1: convergences of cost and gradient (top), and kinematic offset (bottom).