Invariant EKF for Map-Aided Localization Using the Raw 3D Lidar Scan Points Directly

Map-aided localization using 3D lidar scan points is an essential and fundamental technology in the field of Intelligent Vehicles (IVs) research, which can estimate the position and orientation of the vehicle in an environment where the Global Navigation Satellite System (GNSS) signal is not available. In recent years, the Invariant Extended Kalman Filter (InEKF) has gradually received attention in the state estimation for autonomous systems. However, how to properly apply the InEKF for the map-aided localization using the raw 3D lidar scan points directly has yet to be explored. In this paper, a novel map-aided localization method based on the InEKF is proposed, in which the raw 3D lidar scan points are directly used to update the predicted state and 3D Registration Algorithms (3DRAs) are employed to estimate the correspondences between the raw 3D lidar scan points and the map points. Then, the Left InEKF (LInEKF) and Right InEKF (RInEKF) equations for the map-aided localization using the raw 3D lidar scan points are derived in detail on the foundation of Lie Group and it is found from the propagation equation that the predictions of the RInEKF are dependent on the location of the map origin. Next, the implementation workflows of both the RInEKF and LInEKF for the map-aided localization are described in detail. Finally, the experiments are conducted on the public datasets and our collected dataset, and the results show that both the LInEKF and the RInEKF can realize the accurate localization when the map size is small and the map origin is near the map, and the LInEKF is more suitable for the map-aided localization than the RInEKF regardless of the location of the map origin and the size of the map.