System Identification under Constraints and Disturbance: A Bayesian Estimation Approach

TL;DR

We address robustly estimating state trajectories and physical parameters of floating-base robots under contact and loop constraints by integrating inverse dynamics as hard equality constraints, dynamically consistent disturbance projections, and energy-based observations within a Bayesian framework. A parameterized equality-constrained Riccati recursion enables linear-time scaling while preserving the problem's structure, and analytical derivatives support efficient gradient-based estimation. The approach delivers faster convergence and more accurate inertial and friction parameters, with improved contact consistency, validated in simulation and on hardware (Unitree B1 with a Z1 arm) and showing tangible benefits for MPC-based locomotion. Overall, the work provides a principled, scalable route to physically consistent joint localization and system identification for complex legged systems.

Abstract

We introduce a Bayesian system identification (SysID) framework for jointly estimating robot's state trajectories and physical parameters with high accuracy. It embeds physically consistent inverse dynamics, contact and loop-closure constraints, and fully featured joint friction models as hard, stage-wise equality constraints. It relies on energy-based regressors to enhance parameter observability, supports both equality and inequality priors on inertial and actuation parameters, enforces dynamically consistent disturbance projections, and augments proprioceptive measurements with energy observations to disambiguate nonlinear friction effects. To ensure scalability, we derive a parameterized equality-constrained Riccati recursion that preserves the banded structure of the problem, achieving linear complexity in the time horizon, and develop computationally efficient derivatives. Simulation studies on representative robotic systems, together with hardware experiments on a Unitree B1 equipped with a Z1 arm, demonstrate faster convergence, lower inertial and friction estimation errors, and improved contact consistency compared to forward-dynamics and decoupled identification baselines. When deployed within model predictive control frameworks, the resulting models yield measurable improvements in tracking performance during locomotion over challenging environments.

Figures (22)

• Figure 1: The Unitree B1 quadruped robot performing a step-up maneuver. The leg includes a reduction gear and a four-bar linkage, both explicitly modeled as constraints within our framework. The highlighted cross-section reveals the internal linkage bars, whose inertial properties and actuator-side friction effects are jointly identified to improve physical consistency and model fidelity.To watch the video, click the picture or see \video.
• Figure 2: Overview of the our Bayesian optimization pipeline for system identification. Proprioceptive measurements (e.g., encoders, IMU) and optional exteroceptive measurements (e.g., visual odometry/ICP) are synchronized and used to jointly estimate the state trajectory, disturbances, actuation effects (including friction), and physical parameters. Inertial parameters are represented with a physically consistent parametrization, and process noise is projected onto the constraint-consistent tangent space to respect implicit motion constraints. Dynamic consistency is enforced through explicit inverse-dynamics and motion constraints (contacts, closed-loop kinematics, hybrid/reset events), together with general parameter equality/inequality constraints. Energy-based observations provide additional identifiability of friction by enforcing consistency between actuation power, dissipated energy, and changes in mechanical energy. The resulting structured KKT system is solved efficiently with an equality-constrained Riccati approach, enabling scalable over long, multi-rate datasets and deployment in control pipelines (e.g., MPC).To watch the video, click the picture or see \video.
• Figure 3: Top: Unitree B1 robot traversing a rough terrain. Bottom: Kangaroo robot walking on a sidewalk with an identified model. Planning, control, and estimation for these legged systems require accurately capturing contact dynamics and closed-loop mechanism effects inherent to multibody systems.
• Figure 4: Illustration of Kangaroo's leg kinematic structure and closed-loop modeling. (a) The actual mechanical design of the leg, including the actuator assembly, ankle linkage, and knee bars. The corresponding kinematic graph highlights the multiple closed-loop constraints present in the mechanism, resulting in a connectivity graph with cycles that prevent it from being a tree. (b) Representation of the same leg using an equivalent spanning tree, obtained by opening the closed loops. This process ensures that standard tree-based rigid-body algorithms can be applied.
• Figure 5: Illustration of implicit constraints: (a) Standard four-bar mechanism, showing the loop closure; (b) Mechanism opened at one joint, breaking the loop and reducing the system to a serial chain with two additional constraint forces required to maintain the original motion; (c) Mechanism with one bar split in half, preserving the original degrees of freedom but now requiring two linear constraint forces and an additional torque to enforce the closed-chain kinematics; (d) Planar leg in contact with the ground, the bilateral constraint is modeled exactly the same as in the closed loop mechanism but with the force affecting only one body. The contact frame $\mathcal{F}_{c}$ expressed in local coordinates does not align with the normal of the surface, world coordinates $\mathcal{W}$.