Multisensor robust localization in various environment with correlation checking test

Probabilistic localization based on Bayesian theory has been researched as a sensor fusion method to improve the robustness of localization. Pieces of position information, generated by sensors’ observation models with consideration for noises, are fused according to Bayesian theory. However, having large noises not considered in their observation models, the sensors output erroneous position information; thus, the fusion result has a significant error, even when the other sensors output correct ones. In this research, we have proposed a sensor fusion system with a relative correlation checking test to realize robust localization. Pieces of erroneous position information, biased against others and having a negative correlation with others, are detected and excluded in our proposed system by checking their correlation between all of them. The purpose of this paper is to evaluate the robustness of our fusion system by conducting recursive localization experiments in various environments.

In order to improve robustness of localization, probabilistic localization based on Bayesian theory, such as Kalman Filter, Particle Filter, etc., have been used as a sensor fusion method [5][6][7][8][9][10][11][12][13][14][15][16][17]. It can fuse pieces of position information from every sensor according to the uncertainty of each sensor's observation. However, probabilistic localization is likely to fail when just an uncertainty of a sensor observation is not correctly evaluated. For example, an uncertainty of GPS observation is generally computed according to the value which reflects the number of observable satellites and the arrangement of them, known as Dilution Of Precision (DOP). Although DOP is also used for smartphone position accuracy circle, it cannot reflect the multi-pass error influence, and the GPS position information often contains significantly high error, even when its DOP is low. Many researchers have proposed observation models to evaluate the uncertainty of the observation for each sensor; nevertheless, a general model has not been established due to its high dependency on sensors and environments.
As a different viewpoint from the evaluation of the localization performance using uncertainty, we proposed a sensor fusion system evaluating the localization performance using correlation. Evaluating the correlation of position information with each other, the system aims to detect and exclude erroneous position information biased against others. Our fusion process is as follows; 1.
expressing a piece of position information for each sensor as a discrete probability distribution according to Particle Filter theory, 2. detecting and eliminating probability distributions biased against others by relative correlation checking test, 3. computing a fused distribution by multiplying probability distributions passing the test.
In order to realize robust localization in various environments, we proposed a sensor fusion system with a relative correlation checking test in [12]. Meanwhile, in [13], we combined our proposed system with a Particle Filter for improving robustness our system to a recursive localization method and confirmed the effectiveness in a limited environment. To demonstrate the robustness of our system in various environments, this paper mentions some results of recursive localization experiments in three environments: the road between buildings, parking spaces, square. We show the robustness of localization improves on various kinds of environments.

Related work
Before mentioning our proposed sensor fusion system, we introduce some existing researches classified into three approaches.
The first fusion approach is that a user assigns localization sensors to the environments where a robot moves in, according to their experiences and knowledge [18]. While this approach is done easily if the robot moves a limited area, the workload becomes harder as the area becomes larger. Moreover, there is no guarantee that the assignment will works the best forever because the environment changes as time passes so that this is not suitable from the viewpoint of usability.
The second fusion approach is to compute a piece of position information from all sensors [1,20,21]. For example, in [21], Graeter et al. designed a localization system combining the advantages of a camera and LiDAR: a strong point feature matching of a camera such as Scale-Invariant Feature Transform (SIFT) and VISO, and an accurate depth measurement based on Time of Flight. Their system computes the depth of detected features in images using LiDAR points projected into images. However, in order to achieve the above calculation accurately, it requires strict sensor calibration in the order of millimeter to project LiDAR points into images correctly; moreover, it has to compute the depth of feature points in images by using sparsely projected LiDAR points near them. Even when sensor observations don't contain large noises, this approach sometimes outputs erroneous position information.
The third fusion approach is to compute a piece of position information for each sensor, then fuse all of them. One of the most famous methods of this approach is probabilistic localization according to Bayesian theory.
It expresses pieces of position information as probability distributions, which includes information about position and uncertainty of observation. In this approach, it is a crucial problem how to evaluate an uncertainties in sensors' observation models. As mentioned in introduction, Maier et al. design the GPS observation model which computes its uncertainty based on DOP in [5]; however, it doesn't reflect the multi-pass error influence. Therefore, in [17], Yamazaki et al. proposed an observation model: judging whether a satellite signal is sent by visible satellites or not using satellite arrangement and 3-dimensional map, then removing signals from invisible satellites as reflected one. However, it couldn't consider other causes of GPS localization error: diffraction signals caused in the ionosphere and convection. Because these error sources change unpredictably depending on places and time, the observation model with consideration for all error sources has not been realized yet. On the other hand, as for LiDAR's observation models, Hossein et al. [26] proposed an observation model computing the uncertainty by matching the feature shapes in clustered LiDAR points to landmarks in a three-dimensional map. Akai et al. [10] also proposed an observation model computing the uncertainties by matching LiDAR points to environmental map constructed by Normal Distribution Transition (NDT). However, it would fail evaluation of uncertainty in the case that there are some similar objects in the environment; the uncertainty would be low where a robot is on wrong but similar place to the true position place. There is no standard observation model whatever sensors you use. Erroneous probability distributions fatally affect the fusion result based on the Bayesian theory.
A similar approach to our system was proposed by Wei et al. in [8]. They added a relative sensor evaluation system to Bayesian theory fusion approach. In detail, the similarity between every sensor's position information, expressed as Information Filter, are checked one by one by the normalized innovation squared checking test, then the position information having low similarities with others are removed before the fusion step. All kinds of sensors don't seem to have the same observation noises at the same time and place because their observation characteristics are different from each other so that relative sensor evaluation could detect just erroneous position information from noisy sensors. Information Filter is a Bayesian Filter that assumes the noises in the system follow Gaussian distribution. However, in the real world localization, some noises do not follow Gaussian noises. On the other hand, Particle Filter does not assume the Gaussian distribution. Particle Filter has more flexibility than the information filter because it is not under the assumption of Gaussian noises. Moreover, the experiment in Wei's paper was conducted only in one environment, so it couldn't demonstrate the robustness of localization because the localization accuracy changes depending on the environment. In our works, we proposed a fusion system using Particle Filter and conducted some experiments in various environments to demonstrate our system's robustness of localization in various environment.

Generic probabilistic localization
In this section, we first mention the theoretical basis of the Bayesian state estimation representation. The formulation and mechanisms of the Particle Filter are then explained. At last, the sensor data fusion with multisensor observation is demonstrated.

Theoretical basis of Bayesian theory
The basic theory of the proposed framework is recursive state estimation with sequential observations. The Bayesian state estimation in this work is composed following two steps: Prior update Measurement update where x t , z 1:t , p(·) are the state of the robot at time step t, the observations up to t, the probability density function, respectively. p(z t |x t ) is calculated using sensor data through an observation model. This recursion is initialized with a known probability distribution p(x 0 |z 0 ) based on an initial observation.

Particle filter
Particle filter, which simulates a probability distribution by a set of discrete points (particles), have been used due to its non-parametric features. It approximates p(x t |z 1:t−1 ) in Equation (1) by crude density of a set of particles s [n] t . We denote N is the size of the particles and δ(·) is a Dirac delta function. Equation (1) is approximated by the following function; Then, p(z t |x t ) in Eq. (2) is approximated by an Eq. (4); t where π(·) is an observation model of the sensor. p(x t |z 1:t ) is obtained by substituting Eq. (3) and (4) into Eq. (2) as follows; where, α is a normalizing constant. The Minimum Mean Squared Error (MMSE) or Maximum A Posteriori Probability (MAP) of p(x t |z 1:t ) are commonly utilized to compute an estimated robot position. After all, a new set of particles s [n] t+1 is generated according to p(x t |z 1:t ).

With multi-sensor observation
In probabilistic localization, multiplication of probability distributions is used to fuse sensor estimation. We denote M, p m (·), p f (·) are the number of sensors, a probability distribution from m-th sensor, and a fused distribution; p f (·) is computed according to the following equation.
where α is a normalization factor. The multiplication of a probability is defined as a joint probability. Note that, p(s [n] t |z 1:t ) is represented as p(s [n] ) in the remainder of this paper for simplicity.

Sensor fusion system with relative correlation checking test
During the localization process, the measurements might be contaminated by large disturbances. As mentioned in section I, erroneous probability distribution, biased against others, makes serious influence on a fusion result even though the probabilistic theory is used. In our proposed sensor fusion system shown in Fig. 1, the probability distributions, whose correlations with others are low, are validated as biased in the relative evaluator; then the biased probability distributions are removed in the selector. This section mentions the correlation checking test and a selection process based on relatively checking results. Note that this system can work when there are three or more sensors.

Correlation checking test
The correlations between probability distributions are validated by Pearson correlation [30]. As we denote the covariance between p m (·), p l (·) is Cov[p m (·), p l (·)] , and the standard deviations of p m (·), p l (·) are σ m , σ l respectively, the Pearson correlation efficient ρ m,l is calculated by the following equation; where µ l , µ m are the mean of the p l s [n] , p m s [n] . We utilize ρ m,l as a value representing the coincidence of high probability particles between p m (·) and p l (·).
For example, we consider the probability distributions whose high probability particles are similar as shown in Fig. 2a. The vertical, horizontal axes and blue circles show x , p(·) , s [n] respectively. Both p m (s [i] ), p l (s [i] ) are high at s [i] , and both p m (s [j] ), p l (s [j] ) are low at s [j] . A scatter ) is low and p l (s [j] ) is high at s [j] . Figure 3b shows a scatter plot in the case of Fig. 3a. In Fig. 3b, p l (s [n] ) gradually decrease along with p m (s [n] ) increasing. In other words, p m (s [n] ) and p l (s [n] ) have negative correlation, and ρ m,l is nearly −1 . Therefore, ρ m,l represents the coincidence of high probability particles between p m (s [i] ) and p m (s [j] ) . We can evaluate the correlation by setting the threshold value ρ th to ρ m,l .

Relative evaluation of different sensor observations
At the selection step shown in Fig. 4, the correlation between all probability distributions are evaluated, and the number of ρ m,l > ρ th for each p m (s [n] ) is counted, which is L m . Then, p m (s [n] ) whose L m is lower than M/2 are removed, and other passing probability distribution are fused by Eq. (6). This selective sensor fusion system is on the basis of the following assumption; it seldom happens that different kinds of sensors are affected by the same fatal environmental noises at the same time and place. If the number of erroneous probability distributions is less than half of all, the proposed system can correctly detect and exclude the erroneous probability distributions. On the other hand, if over half of the sensors are affected by environmental noises at the same time, there is the possibility that the proposed system cannot detect only erroneous probability distributions. However, under the above assumption, the peak positions of the erroneous probability distributions would vary with each other because the noises are different depending on the sensor. Thus the erroneous probability distributions would have a negative correlation to others, and this system uses all probability distributions. Although the proposed system cannot decrease the affection of the erroneous position information, it is the same as the conventional system that fuses all probability distribution. Therefore this does not mean the proposed system is inferior to the conventional system. The proposed system is suitable for fusing different kind of sensors. Note that the proposed system must have three or more sensors for the relative evaluation.

Experimental results
We mention localization experiment results in some environments, and discuss the robustness of the proposed system.

Experimental conditions
We conducted three localization experiments at Yakusa campus of Aichi Institute of Technology. The robot (Dongbu Robot TETRA DS-IV) was equipped with GPS (Hemisphere Cresent A101), omni camera (Vstone VSC450U-200-TK), and LiDAR (Hokuyo UTM-30LX) as shown in Fig. 5. Each sensor simultaneously got its data in every 0.2 s. After that, the probability distribution for each sensor is computed, then they are fused by the proposed system and the conventional system, without relative evaluation. We set N = 100, ρ th = 0 and computed estimated positions by MMSE. We measured the true robot's position by using a laser range finder (Leica DISTO S910) set on the environment. The laser sensor was set on the point whose absolute position is acquired by using Google map. The laser sensor can correctly measure the distance between the sensor and the robot. Combining the distance with the relative direction measured by gyro sensor in a laser sensor, the laser sensor can measure the relative position of the robot. The relative position was transformed to the global position using the laser sensor's absolute position.
The observation models of GPS, omni camera, LiDAR are normal distribution model [5], similar picture searching using Bag of Features [31], point cloud matching, respectively, which are described as follows.

Observation model of GPS
As an observation model of GPS, we utilized the normal distribution model expressed by the following equation.
where p GPS and σ GPS and are the GPS estimated position and the standard deviation of normal distribution. This is usually used for the GPS observation model. We reflect DRMS to the standard deviation, which is computed by the multiplication of HDOP and USRE.

Fig. 5 Robot with GPS, omni camera, and LiDAR
HDOP is the degradation value of the measurement accuracy of the horizontal space computed based on the arrangement of satellites. USER is the error value reflected the error cause of the GPS measurement (ionosphere and depending error). USER is set as 1.5 m.

Observation model of camera
As the observation model of the omni camera, similar picture searching of the Bag of Features (BoF) is used.
where L is the number of similar images, p Camera is a position where the picture l is taken. Based on the similarity of similar image sim (l) , the standard deviation of the normal distribution is computed as follows.
Equation (11) is a fitting result of the relationship of the similarity of the images and the distance between the picture position and the robot by non-linear function. The cluster of BoF is created by clustering the feature of Scale-Invariant Feature Transform (SIFT). We set a threshold of the similarity sim (l) to remove the affection of the non-similar images. Therefore, if there is no similar image in the BoF, the probability distribution becomes a uniform distribution.

Observation model of LiDAR
We implemented Point matching algorithm for LiDAR observation model.
where e is the sum of the distances between input points and matched reference points, and σ LiDAR is the standard deviation of the LiDAR observation, set to 1/10 −5 .

Exp. I: The Road between Building
The first experiment was conducted at the road between buildings as shown in Fig. 6. The robot ran on the yellow arrow shown in Fig. 6. The result of localization errors in this experiment is shown in Fig. 7. In Fig. 7, the horizontal axis shows time, and the vertical axis shows the localization errors: the distances between the estimated positions and the true positions. As shown in Fig. 7, the maximum localization error of the conventional system (12)  in the yellow rectangle term was higher than 6 m. On the other hand, the localization errors of the proposed system were lower than 3 m in its term.
The probability distributions from sensors at 330 s obtained by the proposed system are shown in Fig. 8, and Table 1 shows the correlation table at 330 s.
In the distribution images in Fig. 8, gray and white areas represent occupied and unoccupied areas respectively, and the depth of red represents the probability of the robot existence, and green circles show the true positions. Figure 8 show that the LiDAR's and camera's probabilities of particles within 1m from the true position were higher than those of outside particles. On the other hand, the GPS's probabilities of particles within the same area were lower than those of outside particles. This implies the GPS's measurement was contaminated by a large disturbance and should be removed before a fusion step. Table 1 shows that the correlation efficient between the probability distributions from the LiDAR and camera exceeded ρ th = 0 , thus the proposed system fused the probability distributions except the one from GPS. The proposed fusing method succeeded in decreasing the localization errors.

Exp. II: The Square
In the second experiment conducted at the square as shown in Fig. 9, the robot ran on the yellow arrow.
The result of localization errors in Fig. 10 shows the errors of the proposed system in the yellow rectangle term were lower than those of the conventional system. The probability distributions at 152 s in the proposed system are shown in Fig. 11, and Table 2 shows the correlation table at 152 s. Figure 11 shows that the camera's and GPS's particles aside on the true position have higher probabilities than the others. On the other hand, the LiDAR's particles aside on the true position have lower probabilities than the others. This implies the LiDAR's measurement was contaminated by a large disturbance and should be removed. Table 2 shows that the correlation efficient between the probability distributions from the camera and GPS exceeded the threshold ρ th = 0 , and those distributions  Table 1 Correlation table at    have positive correlations. Thus, the proposed system fused the probability distributions except the one from GPS and succeeded in decreasing the localization errors.

Exp. III: The Parking
In the third experiment conducted at the parking space as shown in Fig. 12, the robot ran on the yellow arrow. The result of localization errors in Fig. 13 shows those of the proposed and conventional system were lower than 1 m all times. The probability distributions at 254 s in the proposed system are shown in Fig. 14, and a correlation table at 254 s is shown in Table 3.   Figure 14 shows that the GPS's and LiDAR's particles within 1m from the true position had high probabilities. On the other hand, regarding the particles in the camera, every particle had same probability value, named uniform distribution. Uniform distribution means the sensor cannot identify the specific position. In the parking lot where the experiment III was conducted, the arrangement of the cars were changing. When I made the environmental map, there were many cars. However, on the date when I conducted the localization experiment, there were few cars because it was the day-off of the university. Since the images captured in the localization were not matched to any images captured in the map, the camera could not identify the specific position. So probability distribution was set to a uniform distribution. The uniformed distribution does not affect to the fusion result. However, the localization only by the uniformed distribution would fail because it does not have any position information. Therefore, the proposed system must not select the only uniformed distribution observed by camera. Table 3 shows the proposed system was able to select the probability distributions of the LiDAR and GPS, so that the proposed system succeeded in localizing the same quality as the conventional system fusing all probability distributions.

Consideration
In the environment where one sensor has erroneous position information, the localization errors of the proposed system were less than those of the conventional system. The proposed system successfully reduces the maximum localization errors from 6 m to 3 m. Moreover, in the environment where no sensor has erroneous position information, the localization errors of the proposed system were nearly the same as those of the previous system. It is found that the proposed method can estimate the position with the same or higher accuracy as the conventional method when the pieces of position information from every sensor include erroneous ones. As a result, we confirmed the robustness of the proposed system.

Conclusion
In this paper, we have discussed the robustness of our proposed sensor fusion system by some experiments in various environments. Our proposed system detects and excludes erroneous position information by checking correlations between all of them, and fuses the rest. In all experiments, the localization errors of the proposed system were lower or the same as those of the conventional system. We confirmed the effectiveness of the proposed system. As the future tasks, we plan to fuse other types of sensors and confirm whether or not the robustness improves as the number of sensors increases.