A Robust Tri-Electromagnet-Based 6-DoF Pose Tracking System Using an Error-State Kalman Filter

Magnetic pose tracking is a non-contact, accurate, and occlusion-free method that has been increasingly employed to track intra-corporeal medical devices such as endoscopes in computer-assisted medical interventions. In magnetic pose-tracking systems, a nonlinear estimation algorithm is needed to recover the pose information from magnetic measurements. In existing pose estimation algorithms such as the extended Kalman filter (EKF), the 3-DoF orientation in the S3 manifold is normally parametrized as unit quaternions and simply treated as a vector in the Euclidean space, which causes a violation of the unity constraint of quaternions and reduces pose tracking accuracy. In this paper, a pose estimation algorithm based on the error-state Kalman filter (ESKF) is proposed to improve the accuracy and robustness of electromagnetic tracking systems. The proposed system consists of three electromagnetic coils for magnetic field generation and a tri-axial magnetic sensor attached to the target object for field measurement. A strategy of sequential coil excitation is developed to separate the magnetic fields from different coils and reject magnetic disturbances. Simulation and experiments are conducted to evaluate the pose tracking performance of the proposed ESKF algorithm, which is also compared with standard EKF and constrained EKF. It is shown that the ESKF can effectively maintain the quaternion unity and thus achieve a better tracking accuracy, i.e., a Euclidean position error of 2.23 mm and an average orientation angle error of 0.45°. The disturbance rejection performance of the electromagnetic tracking system is also experimentally validated.