A differential-algebraic extended Kalman filter with exact constraint satisfaction

This work proposes an Augmented Manifold Differential-Algebraic Extended Kalman Filter for state and input estimation for multibody systems described by a set of differential algebraic equations. The proposed Kalman Filter allows for exact constraint satisfaction and does not require reformulation of the multibody system equations. The approach consists of two main steps: (i) elimination of the Lagrange multipliers from the system state via a nullspace projection and (ii) an additional correction step that constrains the a-posteriori estimated system state to lie on the constraint manifold. The latter is achieved by solving a constrained optimization problem using a Euclidean descent approach and projection on the constraint manifold, i.e. manifold optimization. The proposed approach is numerically validated on both open and closed kinematic chain rigid multibody models and is shown to be robust and indifferent to the type of kinematic chain being considered. Furthermore, an experimental validation is carried out where the forces and torques acting on the wheel center of a McPherson car suspension test setup are successfully estimated using a flexible multibody model and the proposed Kalman Filter.