An Adaptive Invariant EKF for Map-Aided Localization Using 3D Point Cloud

Abstract

In map-aided localization using 3D point cloud sets, the poses estimated by 3D Registration Algorithms (3DRAs) are typically fused with other sensor data via the Extended Kalman Filter (EKF) to obtain reliable and smooth results. However, the challenges of this combined method are: 1) The linearization process of EKF may cause errors and singularities due to the state defined by a 6D pose. 2) The results of 3DRA as the measurements of EKF may cause errors in residual calculation. 3) The approach relies heavily on 3DRA to overcome the effects of dynamic scenes. This paper proposes an adaptive localization framework based on Invariant Extended Kalman Filter (Invariant EKF), in which the Lie Group is introduced to define the state. In this framework, the points of the raw point cloud set are the measurements of the filter, and the 3DRA is only employed for data association between the raw 3D point cloud set and the 3D point cloud map. Then, a Concentric Ring Model (CRM) is proposed to reduce the influence of dynamic objects, which can adaptively estimate the covariance of each observed point via Gaussian Process Regression (GPR). Besides, the CRM considers Sensor Measurement Noise (SMN) and Sensor Vibration Noise (SVN). The performance of the proposed framework is evaluated on the KITTI dataset and our dataset. The experimental results show that the proposed method is superior to other state-of-the-art methods, and the CRM can achieve more accurate measurement than ever before, especially in high-dynamic scenes.