The Extended Kalman Filter for Constraint Enforcement in the Forward Dynamics of Multibody Systems

The forward dynamics of multibody systems is widely exploited in engineering applications to simulate the behavior of complex mechanical systems undergoing kinematic constraints. It relies on the integration of the equations of motion of the system. However, the numerical integration error causes the simulation to violate the position and velocity kinematic constraint. In this paper a novel Constraint Enforcement method based on the application of the prediction-correction scheme of the Extended Kalman Filter is proposed and it will be denoted (CE-EKF). First, the equations of motion of the system are integrated to provide thesimulation output estimate, i.e., the prediction. Then, the Kalman Filter computes the correction to ensure that the corrected simulation output is consistent with respect to the constraint equations. Besides proposing such the novel theoretical framework for the application of the Extended Kalman Filter in the suppression of constraints violation. In this work the numerical application of the method is provided for a redundant mechanism. The provided results highlight the effectiveness of the proposed method in comparison with other methodsproposed in the literature.