A Tightly Coupled Multibody Dynamics and Multi-Sensor Fusion Algorithm for Simultaneous Kinematics and Kinetics...

Inertial Measurement Units (IMUs) enable portable, multibody motion capture in diverse environments beyond the laboratory, making them a desirable choice for diagnosing mobility disorders and supporting rehabilitation in clinical or home settings. However, challenges associated with IMU measurements, including magnetic distortions and errors due to integration drift, complicate their broader use for motion capture. In this work, we propose a tightly coupled motion-capture approach that directly integrates IMU measurements with multibody dynamic models via an iterated extended Kalman filter to simultaneously estimate the system's kinematics and kinetics. By enforcing the complete multibody system dynamics and utilizing only accelerometer and gyroscope data, our method accurately estimates joint kinematics and kinetics. Our algorithm is designed to fuse different sensor data, such as optical motion-capture measurements and joint torque readings, to further enhance estimation accuracy. We validated our approach using highly accurate ground-truth data from a 3-degree-of-freedom pendulum and a 6-degree-of-freedom collaborative robot. We demonstrate a maximum root-mean-square difference of 3.75° in the pendulum's computed joint angles with respect to the marker motion-capture inverse kinematics. For the robot, we observed a maximum joint angle root-mean-square difference of 3.24° with respect to the joint encoders, while the maximum joint angle root-mean-square difference of the optical motion-capture inverse kinematics with respect to the encoders was 1.16°. With regard to kinetic estimates, we report a maximum joint torque root-mean-square difference of 3.02 Nm in the pendulum with respect to the marker motion-capture inverse dynamics and 4.27 Nm in the robot relative to its joint torque sensors.