Koopman Operator-based Extended Kalman Filter for Cosserat Rod Wrench Estimation

This paper proposes an observer-based approach for estimating the wrench (force and moment) acting on a 3D elastic rod, using pose states (i.e. robot shape) along the rod as input/feedback. First, the static rod is considered as a dynamical system evolving its states in spatial dimension, Koopman operator theory is adopted to derive an explicit discrete-arclength model for the rod. Then, Extended Kalman Filter is applied to the derived model to estimate wrench states along the rod. Static balance constraints between pose and wrench are enforced to improve force estimation performance. The developed model and wrench estimation approach are evaluated through representative numerical simulations using a single rod. In representative examples, results show average tip force and moment estimation errors of 0.19 N (10.47%), with maximum 0.73 N (31.90%), and 4.52 mNm (2.25%), with maximum 8.28 mNm (7.36%), respectively. Compared to the state-of-the-art, in close test cases, proposed algorithm obtains slightly lower average tip moment and higher force estimation errors of 2.6% and 4.2%, than 2.7% and 2.2%, respectively.