System Identification under Constraints and Disturbance: A Bayesian Estimation Approach

1. Problem Statement

System Identification (SysID) is critical for bridging the Sim2Real gap in robotics, enabling robust locomotion and manipulation. However, conventional SysID methods face several limitations:

• Decoupled Estimation: Traditional approaches often decouple state estimation (localization) from parameter identification. In floating-base systems, errors in model parameters (e.g., inertia, friction) degrade state estimation, while localization errors compromise parameter identification.
• Frequentist Limitations: Standard least-squares or maximum-likelihood methods assume perfect knowledge of state trajectories and often overfit when data is scarce, models are nonlinear, or uncertainty quantification is needed.
• Physical Inconsistency: Many methods neglect complex physical constraints such as closed kinematic loops (e.g., four-bar linkages), bilateral contacts, and discrete events (impacts), leading to biased inertial or friction parameters.
• Friction Observability: Identifying joint friction is difficult, especially in torque-controlled robots lacking direct torque sensors, as friction effects are often confounded with inertial dynamics.
• Scalability: Incorporating complex constraints and Bayesian priors often leads to computationally expensive optimization problems that do not scale to long time horizons.

2. Methodology

The authors propose a unified Bayesian System Identification framework that jointly estimates state trajectories, physical parameters, and disturbances while enforcing physical consistency.

A. Bayesian Optimization Formulation

The problem is formulated as a constrained optimization minimizing a cost function composed of:

• Priors: Gaussian priors on initial state, parameters, and process noise.
• Observation Likelihood: Terms for proprioceptive measurements (encoders, IMU) and energy-based observations.
• Hard Constraints:
  • Dynamics: Hybrid inverse dynamics (forward dynamics is avoided due to numerical instability).
  • Constraints: Equality and inequality constraints for bilateral contacts, closed kinematic loops, and parameter validity (e.g., mass positivity, symmetry).
  • Disturbance Projection: Process noise is projected onto the tangent space of the constraint manifold to ensure dynamically consistent uncertainty.

B. Hybrid Dynamics with Closed-Loop Mechanisms

• Implicit Constraints: The framework models robots with closed kinematic loops (e.g., four-bar linkages) and bilateral contacts using implicit motion constraints ($\phi(q)=0$).
• Analytical Derivatives: The authors derive analytical derivatives for the KKT systems governing both continuous modes (acceleration-level constraints) and discrete reset maps (impact laws). This allows for efficient gradient-based optimization.
• World-Aligned Frames: For bilateral contacts, constraints are projected into world-aligned frames to correctly enforce friction cones and impulse-momentum laws.

C. Parameterization and Physical Consistency

• Inertial Parameters: Uses an Exponential-Eigenvalue (EE) parametrization. This maps unconstrained coordinates to physically valid inertial parameters (ensuring positive mass, positive definite inertia matrices, and triangle inequalities on principal moments) via exponential maps.
• Friction Modeling: Employs a smooth, differentiable friction model capturing stiction, Coulomb, viscous, and Stribeck effects. Constraints are enforced to ensure friction always dissipates energy.
• Energy-Based Observations: To improve friction identifiability without direct torque sensors, the framework incorporates an energy balance equation:
  $$\int v^\top \tau dt = \Delta T_{mech} + E_{dissipated}$$
  This couples actuation power with changes in mechanical energy and frictional dissipation, providing an additional signal to disambiguate inertial and friction parameters.

D. Scalable Solver: Parameterized Riccati Recursion

To solve the large-scale optimization problem efficiently:

• The authors derive a parameterized equality-constrained Riccati recursion.
• This approach exploits the Markovian structure of the dynamics and the sparsity of the problem.
• It achieves linear complexity ($O(N)$) with respect to the time horizon, preserving the banded structure of the KKT system.
• It handles stagewise equality constraints (dynamics, contacts) and parameter constraints (symmetry, bounds) simultaneously without increasing asymptotic complexity.

3. Key Contributions

1. Unified Bayesian Framework: A single optimization program that jointly estimates states, parameters, and disturbances, avoiding the fragility of decoupled pipelines.
2. Constraint-Aware Dynamics: Explicit integration of inverse dynamics, closed kinematic loops, bilateral contacts, and discrete reset maps with analytical derivatives.
3. Physically Consistent Parameterization: Use of EE parametrization for inertial parameters and constrained optimization for friction to guarantee physical plausibility (e.g., energy dissipation).
4. Energy-Based Observability: Introduction of energy-based residuals to significantly improve the identifiability of friction parameters in the absence of direct torque sensing.
5. Scalable Solver: A novel Riccati recursion solver that handles equality constraints and parameter coupling with linear time complexity, enabling application to long, high-frequency datasets.

4. Results

The framework was validated through extensive simulations and hardware experiments on a Unitree B1 quadruped equipped with a Z1 arm.

• Forward vs. Inverse Dynamics: Inverse dynamics formulations converged faster and yielded lower trajectory and parameter errors compared to forward dynamics, particularly in contact-rich scenarios.
• Impact of Constraints: Enforcing parameter constraints (e.g., left-right symmetry, total mass) significantly reduced inertial parameter errors without degrading trajectory fit.
• Energy Observations: Including energy-based observations reduced friction estimation errors by up to 50% across various robotic platforms (biped, quadruped, manipulator) with only a marginal increase in trajectory error and iteration count.
• Comparison with Baselines: The proposed method outperformed classical frequentist inverse-dynamics regressors and energy-based regressors under increasing Gaussian noise, demonstrating superior robustness.
• Hardware Validation:
  • Successfully identified parameters for the B1's complex four-bar leg mechanism, resolving torque prediction errors that existed with nominal URDF parameters.
  • The identified model enabled improved end-effector tracking during manipulation and better centroidal momentum tracking during stair climbing compared to models using nominal parameters.

5. Significance

This work represents a significant advancement in robotic system identification by unifying physical consistency, constraint handling, and Bayesian estimation into a scalable framework.

• Robustness: It addresses the "Sim2Real" gap by producing models that respect physical laws (energy conservation, friction limits) and handle sensor noise and model uncertainty rigorously.
• Applicability: The ability to identify complex mechanisms (like four-bar linkages) and friction without force/torque sensors makes it highly applicable to modern, cost-effective legged robots.
• Control Integration: The resulting models are directly compatible with Model Predictive Control (MPC) and learning-based controllers, leading to measurable improvements in locomotion performance over challenging terrains.

In summary, the paper provides a principled, scalable, and physically rigorous approach to system identification that overcomes the limitations of traditional decoupled and frequentist methods, enabling more agile and robust robotic behaviors.