⚡ electrical engineering

Formulating Multibody Models for Moving Horizon Estimation

This paper presents a multibody-focused formulation of the moving horizon estimation problem that accommodates various model representations from ODEs to index-3 DAEs, demonstrating through comparative analysis that direct constraint handling in index-3 formulations offers superior computational efficiency while index-2 formulations provide an easily implementable alternative for enforcing kinematic constraints on both position and velocity levels.

Original authors: Andrea Stasi, Arnim Kargl, Peter Eberhard, Henrik Ebel

Published 2026-09-01

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Andrea Stasi, Arnim Kargl, Peter Eberhard, Henrik Ebel

Original paper licensed under CC BY 4.0 (https://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

In the world of engineering, knowing exactly where a machine is and how it is moving at any given moment is often the difference between smooth operation and catastrophic failure. Whether it is a robotic arm assembling a car, a vehicle navigating a bumpy road, or a biomechanical system mimicking human movement, engineers rely on a constant stream of data to make decisions. However, sensors are never perfect; they are noisy, they can be ambiguous, and they often cannot see the internal parts of a machine that are crucial for control. To fill these gaps, engineers use mathematical tools called state estimators. These tools act like a sophisticated guesser, combining a computer model of how a system should behave with the imperfect data coming from real-world sensors to produce the best possible picture of the system's current reality. For decades, a method known as the Kalman filter has been the industry standard for this task, but it struggles when systems become highly complex, non-linear, or when they are bound by strict physical rules that cannot be broken.

A team of researchers from Lappeenranta-Lahti University of Technology and the University of Stuttgart has tackled this challenge by developing a new way to build these estimators for complex mechanical systems. They focused on a technique called Moving Horizon Estimation, which looks at a sliding window of recent data rather than just the current moment, allowing it to handle complex, non-linear behaviors much better than older methods. The core of their work involves figuring out how to feed the raw, complex mathematical descriptions of these machines—specifically those involving rigid bodies connected by joints—directly into the estimation process without simplifying them too much. In their study, they tested several different ways of formulating these problems using computer simulations of systems like a pendulum on a cart, a five-bar linkage, a delta robot, and a flexible robotic arm.

The researchers discovered that the most common approach, which involves simplifying the complex mathematical models into a form that is easier to solve but less accurate, actually leads to significant problems. When they tried to use these simplified models, the estimated positions of the machine parts began to drift away from physical reality. In one specific test involving a pendulum on a cart, the simplified model allowed the pendulum's rod to stretch by nearly five millimeters, a violation of the laws of physics that would render the data useless for precise control. Even when they added a standard fix to this simplified model, a small but noticeable error remained. In stark contrast, when the researchers fed the original, complex mathematical description directly into the estimator, the system satisfied the position constraints perfectly, with errors so small they were effectively zero. However, the study noted a nuance: while the position constraints were met to numerical precision, the velocity-level constraints in this direct method still showed non-negligible violations, meaning the estimated speed of the parts did not always perfectly align with the physical rules of motion.

Beyond accuracy, the study revealed a surprising finding regarding speed. Conventional wisdom suggests that solving the most complex version of a problem takes the most time. However, the researchers found that the direct, complex approach was actually the fastest. The simplified models, which were thought to be computationally lighter, required significantly more time to solve—taking over five seconds in some cases compared to less than one second for the direct method. This happened because the simplification process created mathematical expressions that were incredibly difficult for the computer to calculate repeatedly. The direct method, by keeping the structure of the physical model intact, allowed the computer to solve the problem much more efficiently.

The team also compared different ways of handling the mathematical constraints that keep the machine parts connected. They found that a specific formulation, known as the index-2 approach, allowed them to enforce rules on both the position and the speed of the machine parts simultaneously and accurately. This is crucial because a machine that is in the right place but moving at the wrong speed is just as problematic as one in the wrong place. The study showed that this method could be implemented without needing to manually tune dozens of parameters, making it possible to automatically generate highly accurate estimators directly from the machine's design model.

Ultimately, the work demonstrates that for complex mechanical systems, trying to simplify the math to make it easier to solve is often counterproductive. By embracing the full complexity of the physical model and using a method that respects the strict rules of motion, engineers can achieve results that are both more accurate and faster to compute. This approach offers a clear path forward for designing better control systems for advanced robotics and machinery, ensuring that the digital twin of a machine stays perfectly aligned with the physical reality it represents.

Technical Summary: Formulating Multibody Models for Moving Horizon Estimation

Problem Statement
State estimation for multibody systems (MBS) is challenging due to significant nonlinearities and the prevalence of kinematic constraints, which often result in high-index (typically index-3) differential-algebraic equations (DAEs). Traditional state estimation approaches, such as the Kalman filter and its variants (EKF, UKF), are predominantly designed for ordinary differential equations (ODEs). Adapting these methods to DAEs often requires index reduction and constraint stabilization techniques (e.g., Baumgarte stabilization), which introduce additional tuning parameters, obscure theoretical properties, and fail to guarantee constraint satisfaction up to a configurable numerical precision. This lack of strict constraint enforcement can render subsequent applications, such as Model Predictive Control (MPC), infeasible if the estimated state violates kinematic constraints. Furthermore, existing literature lacks a systematic comparison of how different multibody model formulations (ranging from ODEs to index-3 DAEs) integrate with Moving Horizon Estimation (MHE), a framework capable of handling nonlinearities and constraints natively.

Methodology
The authors propose a framework for formulating Moving Horizon Estimation (MHE) problems that directly incorporates multibody models without resorting to ad-hoc index reduction or stabilization parameters. The core of the methodology involves defining a general Optimal Estimation Problem (OEP) template that accommodates implicit discretization schemes and algebraic constraints.

The study investigates and compares four distinct formulations for integrating multibody dynamics into the MHE framework:

Index-3 DAE: The original high-index formulation where kinematic constraints ( $c(q)=0$ ) are handled directly by the optimization algorithm. Lagrange multipliers are treated as optimization variables. Special care is taken to avoid overconstraining the problem by selectively enforcing constraints on position and velocity levels depending on the integration scheme (e.g., Radau IIA).
Index-2 DAE (Gear-Gupta-Leimkuhler - GGL): An index-reduced formulation that introduces additional Lagrange multipliers to enforce constraints on both position and velocity levels explicitly within the optimization problem.
Index-1 DAE: A formulation using Baumgarte stabilization to enforce constraints at the acceleration level, requiring tuning of stabilization coefficients ( $\alpha, \beta$ ).
ODE Formulation: A reduction of the DAE to an ODE by differentiating constraints twice and embedding them into the dynamics, also utilizing Baumgarte stabilization.

These formulations are tested using two discretization schemes:

Euler Methods: Both implicit (with and without internal root-finding) and explicit.
Radau IIA: A two-stage implicit Runge-Kutta scheme chosen for its accuracy in handling DAEs.

The performance of these combinations is evaluated across four simulated multibody systems: a pendulum-on-a-cart, a five-bar linkage, a delta robot, and a flexible serial manipulator. Both minimal (generalized) and redundant (absolute Cartesian) coordinate formulations are analyzed. The simulations utilize the Neweul-M2 toolbox for symbolic equation generation and CasADi with the IPOPT solver for numerical optimization.

Key Contributions

Direct DAE Integration: The paper demonstrates that MHE can be formulated to directly handle index-3 DAEs by treating Lagrange multipliers as optimization variables, thereby eliminating the need for index reduction or stabilization tuning parameters.
Comparative Analysis: It provides the first systematic comparison of MHE formulations for multibody systems, covering index-3, index-2 (GGL), index-1, and ODE variants across different integration schemes.
Constraint Satisfaction: The study highlights that direct index-3 and index-2 formulations satisfy kinematic constraints up to the numerical precision of the optimizer, whereas index-reduced (ODE/Index-1) formulations exhibit non-negligible constraint drift even with stabilization.
Computational Efficiency: Contrary to the intuition that reducing model complexity (e.g., to ODEs) improves estimation speed, the results show that direct index-3 formulations often yield the lowest computational costs while maintaining the highest constraint accuracy.

Results

Constraint Violation: In a pendulum-on-a-cart example, the direct index-3 formulation achieved constraint violations on the order of $10^{-8}$ mm, whereas the ODE formulation with Baumgarte stabilization showed violations up to 5 mm. The index-2 (GGL) formulation successfully enforced constraints on both position and velocity levels throughout the horizon.
Computational Cost: The ODE formulation, despite having the fewest optimization variables in some cases, proved to be the most computationally expensive due to the complexity of the symbolic expressions required for the augmented mass matrix inversion. The index-3 formulation generally required the least solution time among DAE-based approaches.
Coordinate Choice: Minimal-coordinate formulations generally resulted in faster solution times compared to redundant coordinate formulations, with the exception of the flexible manipulator where the complexity of the minimal-coordinate kinematics outweighed the benefit of fewer variables.
Discretization: Multi-stage Radau IIA schemes required more time than Euler methods due to the increased number of variables but offered higher accuracy. The implicit Euler implementation using an internal root-finder was found to be highly inefficient and unreliable for larger models.

Significance and Claims
The authors claim that this work serves as a practical guideline for generating nonlinear state estimators from multibody models with minimal manual tuning. By leveraging the inherent ability of MHE to handle constraints, the proposed direct index-3 and index-2 formulations allow for the automatic generation of estimators that satisfy kinematic constraints to a configurable accuracy. This is particularly significant for applications where the estimated state serves as an initial condition for MPC, as constraint violations in the estimate can lead to infeasible control problems. The paper concludes that approaches successful for simulation (like index reduction) are not necessarily optimal for estimation, and that retaining the high-index structure within the optimization problem yields superior computational efficiency and constraint satisfaction. Future work is identified as assessing these formulations on real-world hardware.

More like this