Fault tolerant multi-sensor fusion based on the information gain

. In the last decade, multi-robot systems are used in several applications like for example, the army, the intervention areas presenting danger to human life, the management of natural disasters, the environmental monitoring, exploration and agriculture. The integrity of localization of the robots must be ensured in order to achieve their mission in the best conditions. Robots are equipped with proprioceptive (encoders, gyroscope) and exteroceptive sensors (Kinect). However, these sensors could be aﬀected by various faults types that can be assimilated to erroneous measurements, bias, outliers, drifts,... In absence of a sensor fault diagnosis step, the integrity and the continuity of the localization are aﬀected. In this work, we present a muti-sensors fusion approach with Fault Detection and Exclusion (FDE) based on the information theory. In this context, we are interested by the information gain given by an observation which may be relevant when dealing with the fault tolerance aspect. Moreover, threshold optimization based on the quantity of information given by a decision on the true hypothesis is highlighted.


Introduction
In literature, several research deal with the localization of multi-robot system.Initially, localizing a group of robots was done by estimating independently the N robots poses without taking into account useful contributions from the other robots.This approach, based on the odometer measurements, suffers from errors accumulations.To deal with this limitation, collaborative localization could be applied.
In [1], the collaborative localization is formulated through a distributed architecture based on the use of the Kalman Filter (KF).This approach takes advantage of all the information in the robots team.The estimation procedure is distributed among N filters, one for each robot.However, during the update step, when one robot observes another one using its exteroceptive sensors, relative position is measured and all robots should communicate and update their covariance matrices.The localization accuracy is achieved at the cost of high computational requirement.
Collaborative localization could be done through in a decentralized architecture as the method presented in [2] where the covariance intersection algorithm is used to fuse estimates with unknown correlations.This method avoid the calculation of cross correlations terms between robot pose estimates but at cost of lower precision.Other decentralized architectures ignore the cross correlations terms which may lead to inconsistent estimation as in [3].In [4] synchronous communication between the robots must be ensured.
In literature, the analytical redundancy FDE approaches are divided into model based method (KF, parity relation. . .), data driven method (artificial neural network, Bayesian belief networks. . . ) and knowledge based expert system [5].Moreover, endogenous fault detection [6] refers to the case when robot is able to detect faults in itself and exogenous fault detection [7] refers to the case when robot detects faults in another one leading to a more robust system.
Fault detection could be done in a centralized architecture as in [8] where information is sent to a central FDE unit or in a distributed architecture as in [9].The limitations of the centralized architecture appear in the sensitivity of the system to a single point-failure (of the central unit), the high computational requirement on the server and the problem of scalability to a larger team size.
In [10], a layered fault detection and exclusion is presented where the faults are divided into two types: those that could be monitored on the robots (this type of default should be modeled) and those that could be detected using the collaborative team after taking into account redundant measurements.
In this work, the aim is to develop a method able simultaneously to localize accurately the group of robots and to detect and to exclude the faulty sensors measurements from the team.The method is based on the sate estimation using the Information Filer (IF) which is the informational form of the KF.The main advantage of this filter appears in the update step modeled as a simple summation between the information contributions of different observations.Therefore, a distributed architecture for the data fusion and for the FDE step can be developed through this filter.On the other hand, residuals tests based on the divergence between the priori and the posteriori distributions of the IF are generated to detect and to exclude the faulty sensors measurements.This divergence is calculated in term of the Kullback-Leibler divergence and it includes two terms which allow to achieve two tests: one acts on the mean and the other on the covariance matrix of the data distributions of the IF.
Threshold optimization is raised in the literature in many research.For example, in some works they propose to define a threshold by fixing the probability of false alarm as in [11] or by fixing the value of the costs of errors as in Bayesian optimization [12].In [13], the threshold is obtained by minimizing or maximizing particular distances.However, these methods do not lead to an optimal threshold because they don't take into account the probability of default and the costs of errors in a rigorous manner.In this work, optimal thresholding method based on the quantity of information is proposed.This method leads to the same rule as in Bayesian optimization but with variable costs values which are function of the prior probability and the probabilities of false alarm and detection.
For the proposed method, we state the following assumptions: (i) N robots are equipped with proprioceptive and exteroceptive sensors, (ii) each robot is able to observe at least one another robot and each robot is at least observed by another one, (iii) communication between the robots is assured (Wi-Fi), (iv) sensors defaults can be assimilated to: erroneous measurement, conflictual measurement, bias, outliers, drifts, sensors freezing...This paper is organized as follows: section 2 provides the collaborative localization using the information filter.The fault detection and exclusion approach based on the Kullback-Leibler divergence is presented in section 3, in addition to the proposed thresholding method.Experimental results are detailed in section 4 followed by a conclusion in section 5.

Collaborative Localization for multi-robot system
The IF is used to estimate the positions and the orientations of the robots.It consists of two steps: • the prediction step using the odometer model • the update step using the relative observations between the robots.
The rest of the paper deals with the case of three robots.Note that the method could be generalized to any number of robots.

Prediction step
The state vector of the robots team consists of the state vector of each robot: where: k is the sampling instant is the position and the cap of robot i with respect to a fixed frame.The odometer model is: where: A i k is the input matrix: u i k is the input vector, it consists of the elementary displacement and rotation of robot i obtained from the encoders measurements: k is the process state noise modeled as a white Gaussian noise with zero-mean and covariance matrix The model is non linear, therefore the Extended Information Filter (EIF) is used after linearizing around an estimated value.Consequently, the Jacobian matrix are calculated [14]: The covariance matrix associated to the robot i is: Initially, the covariance matrix of the group of robots is bloc diagonal.When two robots met, they exchange information about their states.Therefore, the cross correlations terms appear in the covariance matrix and they are obtained as follows: The general form of the covariance matrix of the multi-robot system is: In order to use the EIF, the information matrix and the information vector should be calculated:

Update step
When robot i observes robot j using its Kinect, the position of robot j relatively to the frame of robot i is measured: x ji y ji .Likewise, using the gyroscope of robots i and j, and the communication network (Wi-Fi), the orientation of robot j relatively to the frame of robot i is obtained: θ ji .The observation vector is then: where: In general case, the observations vector of the multi-robot system can be written as: The dimension of this vector may varies over time.R ij is the covariance matrix associated to the observation Z ij .The observation model is non linear, its linearizion around the predicted estimation yields the Jacobian: In the case of a state vector X = X 1 k ... X i k ... X j k ... T , the general form of h ji k is: The correction of the prediction is done as a simple summation of the information contributions of each observations: where: n is the number of observations I l (k) and i l (k) are the informations contributions of the observation Z ij on the information matrix and the information vector respectively: Z ij k is the estimated measurement obtained from the predicted state estimation.

Sensors fault tolerance based on the Kullback-Leibler Divergence
In order to ensure the integrity of localization in presence of faulty sensors, a step of FDE should be added.

Fault detection
Consider two probability distributions p(x) and q(x), the Kullback-Leibler Divergence is defined as: log is the natural logarithm The divergence between the Gaussian distributions obtained in the predicted step of the EIF and the Gaussian distributions obtained in the corrected step is called the Global Kullback-Leibler Divergence (GKLD) and is given by [14]: M is the dimension of the state vector: in the case of three robots, M = 9.Equation 24 could be interpreted as the summation of two terms: ) assimilated to the Mahalanobis distance, it allows a test on the mean (ii) log )−M assimilated to the Burg matrix divergence [15], it allows a test on the covariance matrices.
The GKLD measures the divergence between the distribution obtained in the predicted step of the EIF and the one obtained in the update step.It could also be interpreted as the information surprise given by the observations.After the convergence of the posteriori distribution (obtained from the update step) to the priori distribution (obtained from the predicted step), this surprise should not exceed a given value.
Therefore, this residual is sensitive to sensors defaults occurring in the predicted or the update step of the EIF.

Threshod optimization
The comparison of the GKLD to a threshold value is a vital step in order to take the decision about the presence of faulty sensors in the robots team.In this work, an optimized thresholding method based on the quantity of information given by a decision is proposed.
The obtained threshold benefits from several advantages: • it deals with the problem from an informational point of view: the optimal threshold maximizes the quantity of information provided by a decision on the true hypothesis • fixing a probability of false alarm isn't required • the calculation of the costs of errors (costs of choosing hypothesis i when hypothesis j is true) is done in an optimized manner contrary to the case of Bayesian optimization.
Figure 1 presents the probabilities associated with different decisions that could be taken by the detector (u i ) given the true hypothesis (H i ).The decision problem deals with two hypotheses: H 0 represents the null hypothesis (absence of faulty sensors), H 1 represents the alternative hypothesis (presence of faulty sensor).Therefore, we define: • The detection probability: is the probability of choosing H 1 when H 1 is true: • The false alarm probability: is the probability of choosing H 1 when H 0 is true: • The miss detection probability: is the probability of choosing H 0 when H 1 is true: Consider f (GKLD/H i )(i = 0, 1) to be the probability d ensity f unction (pdf ) of the GKLD under the hypothesis H i , the detection problem is reduced to: An example of these data distributions is given in figure 2 where: λ is the detection threshold.In Bayes detection theory, the choice of threshold is based on minimizing a risk function [16].The costs of choosing hypothesis H i (i = 0, 1) when H j (j = 0, 1) is true are supposed to be known and constant.In this work, the threshold optimization is formulated using the mutual information between the decision and the hypothesis I(H, u) [12]: with: The minimum of I(H, u) is achieved when P F = P D , indicating that the decision does not provide any information about the true hypothesis.However, maximizing the mutual information is equivalent to maximizing the information gain given by the decision on the true hypothesis [17].
Consequently, the maximization of I(H, u) leads to a likelihood ratio test expressed in function of the prior probability (P (H i )), the probability of false alarm and the probability of detection: where: and is the cost of choosing H i when H j is true.
The detection test could be expressed directly in function of the GKLD: For a given value of P 0 , the optimal threshold λ is obtained at the values of P F and P D that maximize the mutual information.

Fault exclusion
After the detection step, the faulty sensors should be excluded from the fusion procedure.
In order to ensure the exclusion of faults in the encoders, the gyroscope or the Kinect, we propose to divide the observation vector into two parts: Z ji,xy = x ji y ji T and Z ji,θ = θ ji .
Using H ji , we can conclude the observations matrix associated to the observations Z ji,xy (H ji,xy ) and the one associated to the observations Z ji,θ (H ji,θ ): And: When robot i observes robot j, two residuals (KL ji,xy and KL ji,θ ) using the state vector of the observed robot are introduced.KL ji,xy is obtained from a filter denoted EIF ji,xy in such way that only the observation Z ji,xy is used in the update step of the EIF.KL ji,θ is obtained from a filter denoted EIF ji,θ designed in such way that only the observation Z ji,θ is used in the update step: where m is the dimension of the corresponding state vector: m = 3 P jj,xy k/k , X j,xy k/k are respectively the covariance matrix and the state vector of robot j obtained when the observation Z ji,xy is used in the update step. Similarly: KL ji,xy is influenced by a default in the encoders of robot i and j, in addition to a default in the Kinect of robot i. KL ji,θ is influenced by a default in the encoders or the gyroscopes of robots i and j.Consequently, the isolation step becomes feasible after creating a set of residuals [KL ij,xy , KL ij,θ ].
The threshold optimization defined for the GKLD can be applied to the set of KL ij,xy and KL ij,θ in order to obtain a binary vector of 0 and 1.This vector, compared to an appropriate signature matrix, allows the isolation and the exclusion of the faulty sensors from the fusion procedure.
Table 1 presents the corresponding signature matrix which links the residuals to the potential sensors errors, in the case of three robots.As it can be noticed, each default influences a different set of residuals.
Table 1.The signatures matrix: O for odometer, K for Kinect et G for gyroscope.

Results and discussions
The validation of the proposed method is applied in indoor environment using three Turtlebots equipped with encoders, Kinect and gyroscope (figure 3).The divergence of the estimated trajectories relatively to the reference one can be noticed in figure 4.These trajectories are obtained after incorporation of different defaults in the encoders, gyroscopes and Kinect.Figure 5 shows the GKLD used for the fault detection.The threshold λ is calculated after maximizing the mutual information applied on the GKLD distributions in the faulty and non faulty cases.Therefore, f (GKLD/H 0 ) is obtained after using the different sensors in their normal behavior.However, f (GKLD/H 1 ) is concluded from available data obtained while trying to cover different types of sensors defaults.Consequently, for the calculation of f (GKLD/H 1 ), we simulate faults in the encoders, the gyroscope and the Kinect measurements in the form of bias and drifts.The maximum of the mutual information is obtained at P F = 0.0528 and P D = 0.9405 corresponding to a threshold: λ = 0.093.After the fault detection, the faulty sensors should be excluded from the fusion procedure.Therefore, the sets of residuals [KL ji;xy ] and [KL ji;θ ] are calculated as shown in figure 6 and figure 7 respectively.
After finding the distributions of the KL ij,xy (KL ij,θ ) in the faulty and non faulty cases, the maximization of the mutual information applied to each KL ij leads to the optimal threshold value: λ = 1.9924 × 10 −5 corresponding to P F = 0.0121 and P D = 0.9667.
From k = 20 to 50, KL 21,xy and KL 31,xy exceed the threshold value.After comparison with the signature matrix, Kinect 1 is declared faulty.In the same way, we can conclude that the kinect 2 is faulty from k = 100 to 110 and kinect 3 from k = 82 to 90.
From k = 160 to 165 gyroscope 3 is faulty.We note that the gyroscope 2 is faulty at k = 150.Figure 8 proves the effectiveness of the proposed approach after the exclusion of the faulty measurements from the fusion procedure.Indeed, the estimated trajectories of the three robots  are roughly similar to the references.

Conclusion
In this paper, we propose a method able to localize accurately a group of robots with a sensors faults detection and exclusion step.The method is based on a informational framework: information filter and tools of the information theory.The global Kullback-Leibler divergence is used for the fault detection and a set of residuals [KL ij,xy , KL ij,θ ] defined at each time one robot sees another one, are used for the faults exclusion.
Threshold optimization based on the maximization of the quantity of information given by a decision on the true hypothesis is proposed.This method does not require to fix the probability of false alarm nor the costs of errors.
The method was able to localize accurately the group of robots while excluding the faulty sensors measurements.

Figure 2 .
Figure 2. Illustrative example of the probability density functions of the GKLD

Figure 4 .Figure 5 .
Figure 4.The trajectories of the three robots, before the FDE step (in blue) compared to the reference trajectories (in red).