Vision Based Robust Pose Estimation Using Multiplicative Extended Kalman Filter and Iterative Closest Point

Advanced guidance, navigation, and control algorithms are essential tool in autonomous proximity and docking operation, especially for non-cooperative targets such as space debris or end-of-life spacecraft. The relative pose estimation at the high precision level is a prerequisite for successful capture of the target and it is necessitated that various sensors are advisedly incorporated to improve the overall performance. This paper proposes an algorithm for the vision-based relative pose estimation that combines the Multiplicative Extended Kalman Filter (MEKF) and the Iterative Closest Point (ICP) algorithm. Using a point cloud obtained from a Time-of-Flight (ToF) camera, the ICP is first applied to obtain approximate pose of the target. MEKF is then applied to precisely estimate the pose while overcoming the limitation of the standalone ICP. This scheme enables accurate relative pose estimation in various scenarios, especially when the target is non-stationary. To validate the proposed method, numerical simulations have been conducted using the virtual 3D point cloud data. The simulation results show that utilizing the depth information from ToF camera turns out to be effective to continuously provide the estimated pose with improved performance. The RMS errors were maintained within 2.16 arcsec in roll and pitch and 2.524 degree in yaw.