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

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.

💡 Research Summary

The paper addresses a long‑standing challenge in robotic state estimation: the need for accurate process and measurement noise covariance matrices, which are typically unknown and manually tuned. While existing approaches such as adaptive filtering, expectation‑maximization (EM), or learning‑based noise estimation treat these covariances in isolation, they rarely consider the coupling with uncertain kinematic parameters that are especially pronounced in highly dynamic legged platforms.

To solve this, the authors propose a bi‑level optimization framework that simultaneously calibrates noise covariances (Q and R) and kinematic parameters (e.g., foot‑offsets) while embedding the estimator directly in the optimization loop. The lower‑level problem is a full‑information MAP estimator (FIE) that, given a set of covariance and kinematic parameters, computes the optimal state trajectory (\hat{x}_{0:T}) over a finite horizon. This problem includes the nonlinear process model (IMU‑based dynamics with bias random walks) and the measurement model (joint encoders, IMU, contact sensors) and treats process and measurement noises as slack variables. The upper‑level problem minimizes a differentiable loss function that measures the deviation between the estimated trajectory and ground‑truth data obtained from a motion‑capture system. The loss can incorporate RMSE on position, velocity, and orientation as well as statistical consistency metrics such as NIS and NEES.

A key technical contribution is the use of automatic differentiation to propagate gradients through the lower‑level estimator. By parameterizing covariance matrices via their Cholesky factors, the method guarantees positive‑definiteness while allowing gradient‑based updates. Physical constraints on kinematic parameters (e.g., joint limits) are enforced through projection or barrier terms. The overall optimization is solved iteratively: each outer iteration updates the parameters, each inner iteration solves the MAP problem (using a nonlinear optimizer such as Ceres or GTSAM), and gradients are back‑propagated to refine the parameters.

Experiments were conducted on two platforms: the Unitree Go1 quadruped and a humanoid robot (G1). Both robots were equipped with an IMU, joint encoders, and foot contact sensors, and were operated in a motion‑capture arena that provided high‑precision torso pose and velocity as ground truth. The authors collected diverse motion sequences (trotting, jumping, turning) and compared three configurations: (1) a hand‑tuned EKF, (2) an EM‑based noise estimator, and (3) the proposed bi‑level calibration. Results show a substantial reduction in root‑mean‑square error (RMSE) of the torso pose—approximately 30–45 % lower than the hand‑tuned baseline. Moreover, the calibrated covariances yielded NIS and NEES statistics that fell within the 95 % chi‑square confidence interval for over 90 % of the data, indicating a marked improvement in uncertainty consistency. The calibrated kinematic parameters converged to values within a few millimeters of the nominal design, effectively compensating for manufacturing tolerances and sensor mounting errors.

The authors discuss computational complexity: solving the full‑information MAP problem scales cubically with the horizon length, making real‑time deployment challenging. They suggest possible extensions such as sliding‑window Moving Horizon Estimation (MHE) or GPU‑accelerated solvers to achieve online performance. Additionally, they note that the framework assumes availability of ground‑truth trajectories; future work could explore semi‑supervised or self‑supervised variants that rely on partial observations.

In summary, the paper presents a principled, data‑driven method that unifies noise covariance calibration and kinematic parameter identification within a single optimization loop. By differentiating through the estimator, it directly optimizes trajectory‑level objectives, eliminating the need for manual tuning and achieving superior accuracy and consistency across multiple legged robot platforms. This approach offers a scalable pathway toward robust, plug‑and‑play state estimation for complex robotic systems.