Multisensors (LiDAR/IMU/CAMERA) integrated Simultaneous Location and Mapping (SLAM) technology for navigation and mobile mapping in a GNSSdenied environment, such as indoor areas, dense forests, or urban canyons, becomes a promising solution. An online (realtime) version of such system can extremely extend its applications, especially for indoor mobile mapping. However, the realtime response issue of multisensors is a big challenge for an online SLAM system, due to the different sampling frequencies and processing time of different algorithms. In this paper, an online Extended Kalman Filter (EKF) integrated algorithm of LiDAR scan matching and IMU mechanization for Unmanned Ground Vehicle (UGV) indoor navigation system is introduced. Since LiDAR scan matching is considerably more time consuming than the IMU mechanism, the realtime synchronous issue is solved via a onesteperrorstatetransition method in EKF. Stationary and dynamic field tests had been performed using a UGV platform along typical corridor of office building. Compared to the traditional sequential postprocessed EKF algorithm, the proposed method can significantly mitigate the time delay of navigation outputs under the premise of guaranteeing the positioning accuracy, which can be used as an online navigation solution for indoor mobile mapping.
The establishment of highly efficient, accurate, and lowcost indoor mapping technology is becoming more and more necessary and urgent, due to the growing interest and market of indoor Location Based Services (LBSs). Simultaneous Location and Mapping (SLAM) has become a popular and effective indoor mapping technology in recent years. The SLAM technique is a process of building a map of an unknown environment by traversing it with range sensors while determining the system location on the map simultaneously; it has been explored in robotics and computer science for decades. LiDARbased SLAM is one of the most successful technologies, as it can provide high frequency and high precision range measurements [
Presently, the most popular SLAM algorithms are divided into the following types: Kalman filter, particle filters, and graphbased. Hector SLAM estimates the 3D navigation state based on robust scan matching and inertial navigation system by Extended Kalman Filter (EKF) [
However, most of the existing highly accurate SLAM systems are postprocessed works [
Various methods for the time delay issue have already been addressed in previous works; many efforts concentrate on optimizing the fusion filter [
The details of the LiDAR/IMU fused method were introduced in our previous work [
The proposed LiDAR scan matching algorithm in this paper is an improved, probabilistically motivated Maximum Likelihood Estimation (IMLE) algorithm. It is a brute global optimum search method that can obtain the global optimum position for each scan matching. Aided with IMU, its mapping precision can achieve centimeterlevel with the current postprocessed Extended Kalman Filter (EKF) method [
The rest of this paper is organized as follows: Section
The overview of the traditional sequential LiDAR/IMU integrated system mathematic model is shown in Figure
The mathematical model of the LiDAR/IMU integrated system.
The biases of gyroscope and accelerometer are modeled as a firstorder GaussMarkov process with correlation time
The INS error model with the sensor error models in continuous time can be expressed by
The discrete form of (
The LiDAR and IMU measurements are fused by the EKF algorithm only at the epoch in which LiDAR scan information is obtained. The EKF observation functions are given briefly by
The measurement covariance matrix is written as
The estimates of the EKF prediction functions are
The Kalman gain is
The state vector is updated as
Finally, the estimated error
In the previous sequential IMU and LiDAR fusion model, the next IMU mechanization must wait until the prior LiDAR scan matching has completed. For instance, with the configuration in this system, the time of LiDAR scan matching costs approximately 70 ms. This means that the following IMU data in this 70 ms cannot be processed in time. Thus, the time delay of IMU mechanization due to the processing time cost of LiDAR scan matching is a challenge for online processing.
To settle this issue, a parallel online method is utilized for LiDAR scan matching and the IMU mechanization process. Figure
The parallel processing sequence diagram.
This section will describe mathematical derivation of propagating the error state estimation of EKF from
State transition means using the state in time
Considering
Combining (
According to formulas (
Defining
The covariance matrix associated with
The covariance of
Defining
Combining the above equations yields
To verify the online performance of our proposed integrated system, a series of tests based on the UGV mobile mapping platform has been designed. The hardware and software platform in this paper is what used in previous work called NAVIS [
The detailed parameters of LiDAR and IMU.
LiDAR  IMU  

Product model  Hokuyo UTMEX  Product model  MEMSlevel MTiG 
Sampling frequency  10 Hz  Sampling frequency  200 Hz 
Scan range  0.1 m–30 m  Gyroscope bias  200 degrees/h 
Scan angle  270 degrees  Accelerometer bias  2000 mGal 
Angular resolution  0.25 degrees 
The field tests of the proposed LiDAR/IMU integrated system were conducted along the corridor of Finnish Geospatial Research Institute (FGI) library (see Figure
Top view of FGI library.
As shown in Figure
The static positioning error statistics.
RMS error  Mean error  Maximum error  

PostEKF  North  0.0013 (m)  0.0011 (m)  0.0057 (m) 
East  0.0036 (m)  0.0030 (m)  0.0132 (m)  
Heading  0.2030 (degree)  0.2019 (degree)  0.4243 (degree)  


Online EKF  North  0.0033 (m)  0.0017 (m)  0.0127 (m) 
East  0.0046 (m)  0.0037 (m)  0.0154 (m)  
Heading  0.2319 (degree)  0.2386 (degree)  0.2869 (degree) 
(a) The positioning results with online EKF solution; (b) the positioning results with postEKF solution; (c) the heading results with online EKF and postEKF.
Figure
(a) The map results of UGV platform with LiDAR scan matching standalone. (b) The map and trajectories result of UGV platform with postEKF solution and online EKF solution.
The trajectories of the UGV platform in dynamic mode with postprocessed EKF algorithm and online algorithm are also shown in Figure
The overall dynamic process lasted approximately 176 s. Figure
The dynamic positioning difference statistics between online EKF and postEKF.
RMS error  Mean error  Maximum error  

North  0.0138 (m)  0.0115 (m)  0.0379 (m) 
East  0.0440 (m)  0.0343 (m)  0.0983 (m) 
Heading  0.1979 (degree)  0.1513 (degree)  0.5422 (degree) 
The comparison of positioning outputs by the postEKF solution and online EKF solution.
In addition to the comparison of trajectory accuracy, the map quality also needs to be verified. Figures
(a) NAVIS map result with selected feature points with online EKF solution; (b) NAVIS map result with selected feature points with postEKF solution; (c) TLS reference map and the selected feature points.
The unmovable corners of book shelves and walls are selected as the main feature points for accuracy evaluation. There are in total 67 feature points of corners picked out from the three maps for evaluation. The RMS errors of the selected feature points of the FGI library with the postprocessed EKF solution and online EKF solution are listed in Table
The comparison of accuracy results statistics for the selected feature points.
PostEKF  Online EKF  

Feature points  67  67 
RMS (m)  0.0562  0.0732 
The positioning precision is reflected by the mapping precision indirectly in the indoor environment, due to the lacking of trajectory reference truth. Figure
(a) The cumulative distribution of the mapping errors with postEKF and online EKF; (b) the mapping error drift of the online EKF.
To evaluate the performance of online EKF solution, the time delay of each output epoch is compared with those of postprocessed EKF solution in Figure
The output delay using online EKF solution and postEKF solution.
As the results shown in Figure
The output delay statistics with onlineEKF solution and postEKF solution.
Mean  Maximum  

PostEKF  1.69 ms  88.65 ms 
Online EKF  0.28 ms  3.76 ms 
An online solution for the LiDAR/IMU integrated system is proposed in this paper. The positioning results of IMU and LiDAR scan matching are realtime synchronized using the proposed onesteperrorstatetransition method in the EKF to improve the realtime response of the LiDAR/IMU integrated navigation. The accuracy and online improvement results prove that (1) the proposed online method can achieve the same positioning and mapping accuracy (centimeters level) as the sequential postEKF method; (2) the online EKF solution can reduce the maximum output delay from 88.65 ms in postprocessed EKF to 3.76 ms. It improves the realtime performance effectively. In conclusion, the online solution proposed in this paper can solve the time lag issue of LiDAR/IMU integration without the loss of the positioning accuracy. In the future work, the proposed method will be integrated into embeddedhardware platform and applied for realtime 2D and 3D indoor mapping.
The authors declare no conflicts of interest.
This study was financially supported by the National Key Research and Development Program (nos. 2016YFB0502202 and 2016YFB0501803) and the Academy of Finland (Project: “Centre of Excellence in Laser Scanning Research” (272195)). The authors express thanks to Yuwei Chen, Professor Juha Hyyppä, and other friends in Finnish Geospatial Research Institute, who provided the experiment data and useful feedback.