Pose Estimation of Robotic End-Effectors under Low Speed Motion using EKF with Inertial and SE(3) Measurements

This paper is concerned with pose (position and orientation) estimation of robotic end-effectors under low speed motion where its acceleration is far less than the gravity. An Extended Kalman Filter (EKF) is designed to fuse measurements from an inertial measurement unit (IMU) and a SE(3) measurement system. The IMU consists of a tri-axis accelerometer and a tri-axis gyroscope that sense the end-effector acceleration and angular velocity. The SE(3) measurement system tracks the absolute position and orientation of the end-effector. The filtering scheme has two features. Firstly, the IMU's acceleration measurement, which is dominated by the gravity, is used as observation instead of input for better state estimation. Secondly, it efficiently merges the end-effector orientation measurement with its prediction. Experimental results show the effectiveness of the proposed filter.