Information Fusion 37 (2017) 61–76
Contents lists available at ScienceDirect
Information Fusion journal homepage: www.elsevier.com/locate/inffus
Multi-sensor fusion approach with fault detection and exclusion based on the Kullback–Leibler Divergence: Application on collaborative multi-robot system Joelle Al Hage∗, Maan E. El Najjar, Denis Pomorski CRIStAL laboratory (Research Center in Computer Science, Signal and Automatic Control of Lille) CNRS UMR 9189, University of Lille, Villeneuve d’Ascq, France
a r t i c l e
i n f o
Article history: Received 16 March 2016 Revised 14 December 2016 Accepted 11 January 2017 Available online 12 January 2017 Keywords: Multi-sensor data fusion Kullback–Leibler Divergence Fault detection and exclusion Threshold optimization Information filter Collaborative Localization
a b s t r a c t This paper presents a multi-sensor fusion strategy able to detect the spurious sensors data that must be eliminated from the fusion procedure. The used estimator is the informational form of the Kalman Filter (KF) namely Information Filter (IF). In order to detect the erroneous sensors measurements, the Kullback– Leibler Divergence (KLD) between the a priori and a posteriori distributions of the IF is computed. It is generated from two tests: One acts on the means and the other deals with the covariance matrices. Optimal thresholding method based on a Kullback–Leibler Criterion (KLC) is developed and discussed in order to replace classical approaches that fix heuristically the false alarm probability. Multi-robot systems became one of the major fields of study in the indoor environment where the environmental monitoring and the response to crisis must be ensured. Consequently, the robots required to know precisely their positions and orientations in order to successfully perform their mission. Fault detection and exclusion (FDE) play a crucial role in enhancing the integrity of localization of the multirobot team. The main contributions of this paper are: - developing a new method of sensors data fusion that tackle the erroneous data issues, - developing a Kullback–Leibler based criterion for the threshold optimization, - Validation with real experimental data from a group of robots. © 2017 Elsevier B.V. All rights reserved.
1. Introduction Since the end of the last century, multi-sensor data fusion has received a significant attention in several fields. However, several issues may arise when fusing information from multiple sources, such as the data imperfection, correlation, inconsistency and disparateness [1]. One of the major problems in the sensors fusion is that the provided measurements may be spurious leading to an inaccurate estimation. Therefore, these measurements should be detected and isolated from the fusion procedure [2,3,4]. In literature, 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]. In this work, a distributed FDE method based on a bank of Extended Information Filter (EIF) [6] with residual tests taking the form of the KLD [7,8] is presented. IF is the informational form of the KF; it deals with the information vector and the information matrix. It is more advantageous in the correction step than the KF ∗
Corresponding author. E-mail address:
[email protected] (J. Al Hage).
http://dx.doi.org/10.1016/j.inffus.2017.01.005 1566-2535/© 2017 Elsevier B.V. All rights reserved.
[9] and it allows a distributed or decentralized data fusion architecture. The KLD, named also the relative entropy, computes the divergence between two probability density functions (pdfs). In the case of Gaussian distributions, it evaluates the Burg matrix divergence between covariance matrices used in some optimization problems [10,11,12], in addition to the distance between two points inside a space where the covariance matrix is given (this distance is assimilated to the Mahalanobis distance between the means). The KLD could be interpreted as the expected log-likelihood ratio between two data distributions [13]. In this paper, residual tests based on the divergence between the predicted and the corrected distributions of the IF are computed. Statistical study of the fault free KLD test is developed to fit the experimental data to a theoretical pdf. In literature, the fault free residual test is associated to a pdf like for example Chi-Square distribution, T distribution or Fdistribution… in order to perform the threshold calculation on the basis of a required false alarm probability [14,15]. Usually, this false alarm is chosen heuristically or using human expert. Adding also, that these methods do not take into account the distributions of the test statistic in the faulty case. In some applications, the choice of threshold affects largely the system performance justifying the
62
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
need of a suitable methodology for its setting. In this work, an optimized thresholding method based on a KLC is proposed. The KLC is the summation of the information gain between the a priori and a posteriori distributions given a particular value (KLC= KLC0 +KLC1 ). The optimization problem using KLC0 leads to a liberal threshold and that using KLC1 leads to a conservative one [16]. Depending on the application, the users can tune in the interval in order to choose the best threshold adapted to their case. However, the KLC picks optimally a threshold from this range of values. The method is adaptive in the sense that the threshold at instant k takes into account the historical behavior of the system in order to derive the prior probability of the hypothesis using a Maximum Likelihood Estimation (MLE). The framework is applied to a multi-robot system moving in an indoor environment. Robots need to know precisely their positions and orientations to accomplish their mission. In multirobot system, the Collaborative Localization (CL) could be applied [17,18,19] by fusing between the proprioceptive and exteroceptive sensors in order to optimize the positions estimation. One of the existing techniques consist in the fact that each robot detects the others and calculates relative observations leading to an improvement in the localization integrity of the overall system [20,21]. In a damage indoor environment, robots could be affected by various faults type; in absence of a fault detection step, the integrity of the localization procedure is affected. Note that the FDE step can take advantage of the sensors redundancy in the multirobot team to achieve improved reliability of the whole system [22]. This paper is organized as follows: Section 2 presents the problem statement. Section 3 provides the CL using the EIF. Section 4 introduces the KLD for fault detection and exclusion in addition to the statistical study of the residual test. Thresholding using the KLC is detailed in Section 5. Results with data from real experiment are shown and discussed in Section 6, followed by a conclusion in Section 7. 2. Problem statement Faulty measurements inducing erroneous data has been addressed in multi-sensor data fusion in various research. In [23], a hybrid architecture based on the KF and fuzzy logics techniques is proposed in order to develop a fault tolerant multi-sensor data fusion. The N data sensors measuring the same parameter are introduced into N KF. A Fuzzy logic observer is used to supervise each filter and to detect the transient sensor faults. The fusion center applies a voting technique to investigate the presence of persistent sensor fault to be excluded from the fusion procedure. In [4], a fusion method with spurious data detection using Bayesian inference is developed by adding a term representing the probabilistic estimate corresponding to the case that the data is not faulty conditioned upon the data and the true state. An inconsistent sensor increases the variance of the posterior distribution. In [24], a validation sensors data using Bayesian networks is proposed. A probabilistic model of the relations between the variables should be provided by the help of expert or using a learning algorithm. In [25], sensor validation methods based on particular filter for flight control applications are proposed. The sensor system is modeled by a Markov switch dynamic state-space model. In general, the cited methods and others one [3,26] which can be found in the literature require a prior information or a failure model which are not always available for the detection and exclusion of faulty sensors. In addition, most of these methods require a thresholding step which is conventionally performed in a heuristic manner. In this work, a framework for the CL of a multi-robot team with sensors FDE step is presented.
In the literature, CL is addressed in several research studies. Previous approaches on localizing a group of robots were done by solving independently N poses from N robots, ignoring useful contributions from the others in the team. In [27], a CL method for localizing a team of robots is proposed, where one robot moves and the rest of the team form an equilateral triangle. Monte-Carlo localization algorithm was presented in [28] by synchronizing the robot’s belief each time a robot detects another one. The method is carried out within known indoor environments. KF localization is used in [18] in order to deal with the limitation of the previous methods. Other estimation algorithms as the covariance intersection filter [29] and the maximum likelihood [21] are used in CL. CL takes advantage of the relative observations between the robots in order to optimize the positions estimation [20]. It could be done in a decentralized architecture as the one introduced in [19] where the covariance intersection algorithm is used to fuse estimates with unknown correlations; the communication requirement of this CL is O(N). Other decentralized architectures ignore the cross correlation terms between pose estimates which may lead to inconsistent evaluation [30]. In other work, like in [31], a synchronous communication between the robots is required. However, distributed architecture using KF and taking advantage of all the information in the team is presented in [18] where the estimation process is distributed among N filters; improvement in the localization accuracy is achieved at the cost of increased computational requirement. In this paper, each robot performs a self-localization using the measurements from the encoders presenting a bias error that increases over time. The exteroceptive sensors provide the distance and the relative bearing of the observed robot in the reference of the observer; they intervene to correct the predicted positions. An EIF is used to fuse the proprioceptive and the exteroceptive data sensors through a distributed architecture where the predicted position is calculated at each robot and the update could be distributed among robots. IF is a real time data fusion estimator which allows a distributed or decentralized architectures. Note, in the proposed method, all robots can move simultaneously and no map of the internal environment is assumed to be known. In term of fault diagnosis of multi-robot system, previous research on FDE has focused on faults in single robot with redundant localization methods [32] or redundant sensors. In multi-robot system, we distinguish between endogenous fault detection [33] when the robot detects a fault in itself and exogenous fault detection [34] when the robots are able to detect fault in another one leading to a more robust multi-robot system. Fault detection algorithm can be handled in a centralized architecture as in [35] where the information is sent to a central FDE unit or in distributed version as in [36]. The limitations of the centralized approach appear in the high computational load on the server in addition to the problem of scalability to a larger team sizes. In [22], a layered fault detection and isolation method is developed where the faults are divided into two groups: those that could be monitored on a single robot (but these kinds of faults should be modeled) and those that could be detected using the collaborative team. In [36], a distributed fault detection for a team of robots is developed and the advantage of the distributed algorithm versus the centralized one is shown; the method is a training-classification based method . In [37], the fault detection and isolation is based on statistical test that is normally distributed with zero mean in the non-faulty case. In this work, we take advantage of the redundancy in the multirobot system in order to detect and exclude the faulty sensors measurements through a distributed architecture, taking into account the communication among neighboring robots. Residual tests calculated in term of the KLD between the predicted and the cor-
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
63
Fig. 1. The proposed architecture for Fault Tolerant Collaborative Localization.
rected distributions of the EIF are developed in order to distinguish between normal and abnormal sensors behavior. In the literature, under a set of hypotheses: 1-large sample data, 2-diagonal covariance matrix, 3-components independence of the p-dimensional state vector, the KLD is associated to a Chi-square distributions [38]. However, the validation of these assumptions are not always assured. Therefore, in this work, statistical study of the fault free KLD test is developed in order to fit the experimental data with a theoretical pdf. In term of thresholding, as it has been mentioned in the introduction, some of the existing work in the data fusion or in the fault diagnosis, focus on fixing a false alarm probability in order to determine the threshold by considering the test statistic in the fault free case [15,39,40,41,42]. However, ensuring the integrity of localization on the basis of the diagnosis test is an essential goal in this work. Therefore, more elaborate method for the threshold optimization is integrated.
In detection theory, optimal Bayesian operating points was outlined in [43]. It is obtained by maximizing the decision gain yieldf (y/H ) ing to a Likelihood Ratio Test (LRT= f (y/H1 ) ≶ t) between the faulty 0
(f(y/H1 )) and the non-faulty distributions (f(y/H0 )) in function of the prior probability and the costs associated with the different decisions. Mutual information based criterion and Entropy based optimization were introduced respectively in [44] and [45]; similar rule to the one obtained by applying Bayesian criterion is achieved. The main difference of these optimization methods compared with the Bayesian one is that the obtained costs are not constant. In this work, KLC is developed in order to set an optimized threshold value. The KLC can be considered as the summation of two terms, leading to the design of an interval of thresholds. The optimal one lies inside this range and depends on the value of the prior probability of the hypothesis “no fault” (P0 ) estimated using the MLE and the historical system behavior.
64
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
In order to judge the performance of the proposed detection framework, ROC curves of the KLD tests is shown. It’s a plot of the true detection rate versus the false alarm parameterized by the thresholds values. ROC curves which are closer to the upper left corner ((PF , PD ) = (0,1)) correspond to a test with better discriminate results. The slope at any point of the ROC curve is equal to the likelihood ratio between the two probability distributions (the faulty and the non-faulty one). Note that different threshold criteria have been developed with association to the ROC curve. The farthest vertically point from the chance line (The Youden’s index) and the ROC point with minimal distance to (0,1) are from the most popular methods for the threshold calculation [46]. But the obtained thresholds are not the optimal one, since they do not take into account the cost of the errors and the probability of default. For the proposed method, we state the following assumptions:
3. Multi-sensor data fusion for collaborative multi-robot localization
(1) N robots are equipped with proprioceptive and exteroceptive sensors, (2) Each robot is at least able to observe and being observed by another one, (3) Communication between the robots is ensured (Wi-Fi), (4) Sensors faulty measurements can be assimilated to: erroneous measurement, conflictual measurement, bias, outlier’s measures…
The propagation equations of the EIF are described using the encoder measurements. At instant k, the state vector of each robot of the team is considered to be the position (xi , yi ) and the orientation (θ i ), namely the pose of the robot with respect to a fixed frame:
In order to estimate the positions and the orientations of the robots, EIF is used. It is divided into two phases [6]: a) the prediction where the encoder measurements are used in order to derive the evolution model, and b) the update where the relative observations between the robots are used to correct the prediction. When a robot “i” observes a robot “j”, it determines its position and orientation relatively to his frame. These quantities will be used to update the EIF. For clarity, the rest of the paper refers to the case of three robots. Note that our method could be generalized to any number of robots. 3.1. Prediction step
Xki = xik yik θki
T
(1)
The “i” index denotes the robot “i”. The propagation model is as shown below:
The assumption 2 can be simplified while preserving the same level of integrity of the collaborative localization approach by adding more sensors on the robots. In this work, we have preferred to adopt a collaborative architecture. Adding also, the current research orientation is to use several robots to perform a task instead of using a single over-equipped robot. Notice that when the number of robots increases this assumption becomes more realistic. Concerning the assumption 3, in order to perform the collaborative localization and the fault detection and exclusion, in this work, we didn’t address the network communication problem. However, in the case that the communication network is affected, the robots can rely temporary on their odometry in order to perform the localization task without taking advantage of the relative observations between the robots in the team. Fig. 1 presents the distributed architecture of the proposed framework for Fault Tolerant Collaborative Localization (FTCL). The CL part is composed of two sub-parts. The first one consists of the kinematic robot modeling for state space representation using dead-reckoning sensors for each robot. The second one is the multi-robot modeling using relative observations between robots. The CL part permits to set, at each sampling instant k, the dimension of the state and observations vectors. Then an EIF is used to provide robots pose estimation. This part is detailed in Section 3. The FDE part is composed of residual calculation and threshold optimization: At first step, a residual test namely Global Kullback-Leibler Divergence (GKLD) is designed for faults detection. For faults isolation and taking advantage of the simplicity of the correction step of the IF, a bank of EIFij is designed taking into account the relative observations between robots j and i leading to a set of residual KLij . This set is assigned to an appropriate signature matrix permitting the isolation of the faulty sensors measurements in order to be excluded from the fusion procedure using the main EIF. This step is detailed in Section 4. At second step, threshold optimization using the KLC is applied to each residual test in order to find the optimal false alarm and detection probabilities. As it has been mentioned before, the KLC is the summation of two terms and an interval of thresholds could be constructed. This part is detailed in Section 5.
i i Xki +1/k = Xk/k + Aik uik + wik = f Xk/k , uik + wik
(2)
Where: Xki +1/k : is the estimate of Xi at time k + 1 given observations up to time k. Aik is the input matrix
⎛
i cos(θk/k +
⎜ i ⎝ sin(θk/k +
Aik = ⎜
ωki 2
ωki 2
⎞
)
0
)
0⎠
0
⎟ ⎟
(3)
1
uik :
is the input vector, it consists of the elementary displacement and rotation of the robot “i”, calculated from the encoders meaT surements: ui = [i , ωi ] , wik : is the process state noise modeled as Gaussian white noise with zero mean and covariance matrix Qki . Since the model is nonlinear, the EIF is applied. Therefore, the Jacobian matrices Fki = ∂∂ Xf |X i and Bik = ∂∂ uf |ui are computed: k/k
⎛
1 Fki = ⎝0
i −ik sin(θk/k +
0
cos(θ i k
1 0
⎛0
i cos(θk/k +
⎜
Bik = ⎝
i sin(θk/k + 0
ωki 2
ωki 2
) )
i k/k
+
ωki 2
ωki 2
)
k
⎞
) ⎠
(4)
1 ωi i − 12 ik sin(θk/k + 2k ) 1 2
i ik cos(θk/k +
ωki 2
)
⎞ ⎟ ⎠
(5)
1
The state propagation equation of the global system can be written as in Eq. (6):
1 X X2 X3
Xk+1/k =
+
1
= k+1/k
w1 w2 w3
X X2 X3
+ k/k
A1 0 0
0 A2 0
0 0 A3
1 k
u u2 u3
k
(6) k
Before any update, there is no shared knowledge among the robots. Therefore, the initial covariance matrix of the multi-robot
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
team is block diagonal:
P=
11
P 0 0
0 P 22 0
Where
Pii
0 0 P 33
(7)
is the covariance matrix of the state of robot “i”: T
T
[47]. Once two robots meet, the cross correlation terms appear in the covariance matrix [18]. Thus, its general form after the update step is as shown below:
P=
P 11 P 21 P 31
P 12 P 22 P 32
P 13 P 23 P 33
T
(10)
Yk+1/k = Pk+1/k −1
(11)
And the information vector is as shown in Eq. (12):
T
“j” relatively to the frame of robot “i” (Z ji = [x ji y ji θ ji ] ) is measured: relative distance ρ and relative bearing Zb are obtained from the Kinect measurements, which can be transformed to relative position. The relative orientation is obtained from the gyroscopes measurements. Therefore:
T
cosθki
⎜ γ θki = ⎝ sin θki
= γT
i j θk Xk − Xki
− sin θki
0
cos θki
0
0
1
0
(13)
⎞
0
j yk/k − yik/k−1 −1
0 0
−1 0
j − xk/k + xik/k−1 −1 −1
⎟
I3∗3 ⎠
(17)
↑
Block j
A permutation of these Blocks is needed for different values of i and j. ji nk is the noise in the relative pose measurements. ∂ Zkji
And k = ⎛
1
⎜ kji = ⎜ ⎝0
∂ nkji
:
⎞
⎟ ⎟ ⎠
0
j j i i − sin θk/k xk/k − xik/k−1 + cos θk/k yk/k − yik/k−1 −1 −1 −1 −1
1
j j i i − cos θk/k xk/k − xik/k−1 − sin θk/k yk/k − yik/k−1 −1 −1 −1 −1
0
1
(18)
⎟ ⎠
(14)
(19)
This covariance matrix encapsulates two kinds of noise: the noise on the relative position in addition to the noise on the orientation estimates:
Rkji = Rkji where:
xy
+ Rkjiθ
Rjixy = E njixy njixy k k k
(20)
T
(21)
If the relative positions are calculated from ρ and Zb , then Rk is written as [48]:
ji xy
σρ2 cos2 Zb + ρ 2 σZ2b sin2 Zb ⎜ Rkji xy = ⎝σ 2 − ρ 2 σ 2 sin Z cos Z b b ρ Zb 0
21 ⎞
⎜ y21 ⎟ ⎛ ⎞ ⎜θ 21 ⎟ Z 21 ⎜ ⎟ ⎜ .. ⎟ ⎜ .. ⎟ ⎜ . ⎟ ⎜ . ⎟ ⎟ Z=⎜ ji ⎟ ⎜ x ji ⎟ = ⎜ ⎜ ji ⎟ ⎝ Z ⎠ .. ⎜y ⎟ ⎜ θ ji ⎟ . ⎝ ⎠
T T kji
Rjik = kji E njik njik
⎛
In the case when robot “1” observes robot “2” …, robot “i” observes robot “j” …, the measurements vector can be written as shown in Eq. (15):
x
3∗3
⎞
−1
The covariance of the measurement error is given by:
In order to deal with the odometer inaccuracy and without the deliverance of any absolute positioning data, the possibility that each robot is localized with respect to other robots in the team is considered. When robot “i” observes robots “j”, the pose of robot
⎛
⎜
hkji = ⎝0
ji
(12)
3.2. Update step
⎛
⎛
0
yk+1/k = Yk+1/k Xk+1/k
With:
ji
ji
Finally, the information matrix is the inverse of the covariance matrix of the global system:
θkji
∂ Z ji
error estimation, Hk is the jacobian matrix: Hk = ∂ Xk |Xk/k−1 = i γ T (θk/k ) hkji −1
Block i
j ij Pki +1 = Fki Pk/k F kj /k
(16)
is the measurement estimation, Xk is the pose
(9)
In the propagation step, the cross correlation terms are obtained as in Eq. (10):
Zkji = xkji ykji
ji Zk estimate
where:
(8)
(Qu )ik : is the covariance matrix associated to the input vector
The observation model is non-linear; its linearization around the predicted pose using Taylor expansion yields to the measurement error equation:
Zkji − Zkji estimate = Hkji Xk + kji nkji
ii Pkii+1/k = Fki Pk/k F ik + Bik (Qu )ik Bik + Qki
uik
65
0
σρ2 sin2 Zb + ρ 2 σZ2b cos2 Zb
0⎠ 0
0
where: σρ2 and σZ2 represent the uncertainties in the relative disb
tance and the relative bearing respectively. jiθ On the other side Rk get the form:
− sin θ i x j − xi + cos θ i y j − yi
.. .
Consequently, the dimension of this vector varies over time. The noise associated with the measurement Zji is assumed to be Gaussian white noise with zero mean and covariance matrix Rji .
⎟
(22)
⎛
(15)
⎞
σρ2 − ρ 2 σZ2b sin Zb cos Zb
Rkjiθ =
⎞
⎟ ⎜ ⎝− cos θ i x j − xi − sin θ i y j − yi ⎠ ⎛
1
k/k−1
⎞T − sin θ i x j − xi + cos θ i y j − yi ×σθ2 ⎝− cos θ i x j − xi − sin θ i y j − yi ⎠ 1
(23)
k/k−1
σθ2 is the uncertainty in the orientation estimate.
The correction of the predicted estimations, that appear to be a simple summation between the information contributions of the
66
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
different observations, is done as below (EIF):
Yk/k = Yk/k−1 +
n
Il (k )
(24)
il ( k )
(25)
l=1
yk/k = yk/k−1 +
n l=1
Where n is the number of observations Zij . Il (k) and il (k) are respectively the information matrix and the information-state contribution associated with the observation l (when robot “j” observes robot “i”): T
Il (k ) = H ikj (Rikj )−1 Hki j T
il (k ) = H ikj (Rikj )−1
Zki j − Z ikj estimate
+ Hki j Xk/k−1
(27)
4. Fault detection and exclusion using the Kullback–Leibler Divergence
The KLD of pdf q from pdf p is a non-symmetric measure [49] and could be defined as:
p( X ) q (X )
dX
(28)
Log represents the natural logarithm. In the case of two d-dimensional Gaussian distribution, f(x) and g(x), with mean μ1 and μ2 and covariance matrix P1 and P2 respectively, the KLD is written in the form [50]:
1 P2 KLD( f (x ) g(x ) ) = trace P2−1 P1 + log det 2 P1
− d + (μ1 − μ2 )T P2−1 (μ1 − μ2 )
(29)
This informational metric can be considered as the expected Log Likelihood Ratio (LLR) [13]. It takes into account the Mahalanobis distance, in addition, to the orientation and the compactness of the data distributions represented by the trace and the determinant of the covariance matrix. Particularly, the KLD between the data distribution obtained in the predicted step of the EIF (g(k/k − 1 )) and the one obtained in the corrected step (g(k/k)) has the form given in Eq. (30) and is called the Global Kullback-Leibler Divergence (GKLD) [51]:
GKLD = KLD(g(k/k − 1 ) g(k/k ) )
Yk/k−1 1 1 1 −1 − M = trace Yk/k Yk/k + log −1 Yk/k 2 2 2 T 1 + Xk/k − Xk/k−1 Yk/k Xk/k − Xk/k−1 2
|
|Y
−1 two Gaussian distributions [52]: log |k/k Yk/k |
|
In other term, Eq. (30) is a generalized residual test taking into account different kinds of information between the predicted and corrected distributions. Under a set of assumptions, previous work associated the KLD to a Chi-square distribution [38,53]. According to the conception of our residual test, some of the assumptions may not be verified. Consequently, another study of the data distribution of the KLD is contemplated in this work. The GKLD distribution can be developed by analyzing separately two different tests:
4.1. Fault detection
p (X ) log
|Y
−1 log |k/k − M. This quantity has the form of the Burg matrix Yk/k | divergence [12] and it includes the mutual information between
1)
Diagnosis should be considered in multi-sensor data fusion where highly reliable system is needed. In this work, we propose a distributed FDE method founded on a bank of EIF with tests taking the form of the KLD. There is no need to model every type of fault by using the suggested distributed FDE architecture.
KL( p q ) =
(1) The test on the means when the covariance is unknown:(Xk/k − Xk/k−1 )T Yk/k (Xk/k − Xk/k−1 ). It’s also assimilated to the Mahalanobis distance −1 (2) The test on the covariance matrices: trace(Yk/k Yk/k )+ −1
(26)
Note that the calculation of Il (k) and il (k) could be distributed among the different robots. Therefore, the problem of updating the pose estimates is scalable. Since the information matrix Y is not a block diagonal (after the first time step when two robots met) each measurement will influence the positions of the different robots. A case of study can be found in Appendix A.
Eq. (30) takes the form of a residual test able to detect the presence of faulty sensors in the robots team. M is the dimension of the state vector. This residual could be interpreted as the summation of two terms allowing two kinds of tests:
T1 = n Xk/k − Xk/k−1
Yk/k Xk/k − Xk/k−1
(31)
2 is a Hotelling’s T2 – Test with Hotelling’s distribution TM,n in−1 dexed by two parameters: the dimension M and the degrees of freedom n-1, where n is the number of samples [54]. A relationship connects the Hotelling’s T2 distribution to the Fdistribution:
n−M T1 ∼ FM,n−M M (n − 1 )
(32)
T1 can be viewed as the standard distance between the corrected and the predicted state vector (Xk/k and the Xk/k−1 ). 2)
−1 T2 = (n − 1 ) trace Yk/k Yk/k −1
Yk/k−1 − M + log Yk/k
is approximately distributed as a Chi-Square [55,54] with 12 M (M + 1 ) degrees of freedom: χ 21
For moderated samples size [54],
1−
1 2 2M + 1 − 6 (n − 1 ) − 1 M+1
approximates better the χ 21 2
2
(33)
distribution .
(M (M+1 ))
(M (M+1 ))
T2
(34)
distribution. T2 can be re-
garded as the error of fitting Pk/k to Pk/k−1 . Therefore, in this work, the proposed GKLD is related to the Chi-square and F-distributions. Note, when the number of samples goes to infinity, the F-distribution approximates a Chi-square: 2 /M. FM,n−M → χM The general distribution of the proposed statistical test in the fault free case is given in Eq. (35):
1 GKLD ∼ 2 (30)
T
1 + n−11−
M (n − 1 ) FM,n−M (n − M )n
1 6(n−1 )−1
1 2M + 1 −
2 M+1
χ 2 12 (M(M+1))
(35)
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
67
4.2. Fault isolation After detecting that a faulty measurement is present in the team using the GKLD, the disturbing element should be identified and excluded from the fusion procedure. For this reason, at each time two robots see themselves, a residual test using the state vector of the observed robot, is introduced. In general case, when robot “i” observes robot “j”, this residual is denoted KLji and gets the form:
1 j KL = trace Yk/k 2 ji
−
−1 j Yk/k −1
Fig. 2. Information transmission model.
Y j 1 k/k−1 + log j Y 2 k/k
T j j 1 1 j j j m+ X − Xk/k Yk/k Xk/k − Xk/k −1 −1 2 2 k/k
(36)
where:
Y j = P jj
−1
(37)
and m is the dimension of the considered state vector. For the calj j culation of KLji , the values of Yk/k and Xk/k are obtained from a filter namely EIFji designed in such way that only the observation Zji is used in the correction step. One can remark that KLji depends on: • •
Fig. 3. Thresholds interval obtained using the KLCi criteria.
the encoders of robots “j” and “i”, the measurements of the Kinect of robot “i”.
In the isolation step, the redundancy in the robots team appears clearly by the fact that robots are able to observe and being observed by others one. So, the exclusion of the faulty sensor became feasible and relevant for any robot of the team, after designing a set of residuals KLij (Fig. 1) that will be compared to a signature matrix; this step is shown in the next section. 5. Residual threshold optimization using the Kullback–Leibler Criterion After the construction of the residual tests, thresholding should be applied in order to detect an abnormal behavior in their values and make the right decision about the presence of faulty sensors in the system. Given the GKLD, decision is made among different hypotheses Hi . H0 is the hypothesis of absence of faulty sensors in the multi-robot team and H1 is the hypothesis of presence of faulty sensors.
The prior probabilities are defined as:
p(H0 ) = P0
In Bayesian optimization, the choice of threshold is based on minimizing a mean risk function. The costs associated with this optimization problem are assumed to be known. However, the Neyman-Pearson criterion finds the maximum detection probability without allowing the false alarm probability to exceed a given value. In some applications, it would be interesting to consider the optimization problem in terms of quantity of information as presented in [44] and [45] where they propose to maximize the mutual information or to minimize the conditional entropy of the hypothesis given the decision. We define: PD the probability of choosing H1 when H1 is true:
(38)
Fig. 2 presents the different decisions that could be taken by the detector given the true hypothesis. Starting from the Bayes theorem, we propose to formulate the optimization problem using the information surprise [56] obtained as a measure of change from the prior to the posterior distributions. In this work, we define the KLC as a summation of the KLD between a posteriori and a priori distributions when we observe a particular value [57]:
KLC = KL( p(H/u0 ) p(H ) ) + KL( p(H/u1 ) p(H ) ) where
KL p H/u j
PF the probability of choosing H1 when H0 is true:
PF = p(u1 /H0 ) is the false alarm probability
(40)
p(H1 ) = 1 − P0
5.1. Problem formulation
PD = p(u1 /H1 ) is the detection probability
Fig. 4. The probabilities density functions of the GKLD in the faulty and non-faulty cases.
(39)
where u0 and u1 are the actions of choosing hypothesis H0 and H1 respectively.
therefore:
KLC =
p Hi /u j p( H ) = p Hi /u j log p(Hi ) i={0,1}
p u j /Hi p(Hi )
i
j
p uj
log
p u j /Hi
p uj
(41)
(42)
(43)
68
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
5.1.1. Study of the variation of the KLC: The derivative of the KLC with respect to a given variable v can be expressed as follows:
! 1 − P α ∂αi βi ∂ KLC 0 i = log 2 ∂v ∂v P βi 0 α + β ( i i) i {0,1} " ∂βi P0 βi αi + log ∂v (αi + βi )2 1 − P0 αi
(46)
a) For PF = constant:
∂ KLC P0 α0 = −(1 − P0 ) log 2 ∂ PD 1 − P0 (α + β0 ) 0 P0 α1 +(1 − P0 ) log 2 1 − P0 (α1 + β1 ) P0 β0 1 − PD P0 β1 PD = and = 1 − P0 α0 1 − PF 1 − P0 α1 PF
β0 α0 β1 α1
(47)
Therefore:
∂ KLC = 0 if PD = PF ∂ PD ∂ KLC > 0 if PD > PF ∂ PD ∂ KLC < 0 if PD < PF ∂ PD
Fig. 5. Residual threshold optimization at instant k.
0 KLC = α α+0β log P (αα+ 0 0 0 0 β0 )
β0 β0 log α0 + β0 (1 − P0 )(α0 + β0 ) α1 α1 + log α1 + β1 P0 (α1 + β1 ) β1 β1 + log α1 + β1 (1 − P0 )(α1 + β1 ) +
(44)
β1 = (1 − P0 )PD β0 = (1 − P0 )(1 − PD )
b) For PD = constant:
1 − P ∂ KLC β0 0 = −P0 log ∂ PF P0 (α0 + β0 )2 1 − P β1 0 +P0 log P0 (α1 + β1 )2
where:
α1 = P0 PF α0 = P0 (1 − PF )
Consequently, for a fixed value of PF , KLC is an increasing function on ]PF , 1] and a decreasing function on [0, PF[ with a minimum achieved at PD = PF . KLC is a convex function of PD . Note that we are interested by the interval] PF , 1] (the interval where PD > PF ). Accordingly, for a fixed value of PF , find the threshold that maximizes PD is equivalent to the one that maximizes the KLC.
(45)
1 − P0 P0
The minimum of the KLC is obtained when p(H/u ) = p(H ) involving independence between the hypothesis and the decision. In other term, the decision does not provide any knowledge about the true hypothesis. As it will be shown below, the minimum of the KLC is achieved at the point PD = PF . However, maximizing the information gain between the posterior and the prior distributions is equivalent to maximizing the KLC.
Therefore:
α0 1 − PF 1 − P0 α1 PF = and = β0 1 − PD P0 β1 PD
∂ KLC = 0 if PD = PF ∂ PF ∂ KLC > 0 if PF > PD ∂ PF ∂ KLC < 0 if PF < PD ∂ PF
Fig. 6. Robots in (a) real and (b) simulation environment.
α0 β0 α 1
β1
(48)
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
69
Fig. 7. Trajectories before FDE (blue) compared to the references (red). (For interpretation of the references to colour in this figure legend, the reader is referred to the web version of this article.)
Fig. 9. Data distribution of the GKLD in the non-faulty case f(GKLD/H0 ) (red: theoretical fitting, blue: empirical data. (For interpretation of the references to colour in this figure legend, the reader is referred to the web version of this article.)
Fig. 8. The GKLD for the fault detection Fig. 9.
Consequently, for a fixed value of PD , KLC is an increasing function on ]PD , 1] and a decreasing function on [0, PD [ with a minimum achieved at PF = PD . Note that we are interested by the interval [0, PD [. Therefore for a fixed value of PD , find the threshold that minimizes PF is equivalent to the one that maximizes the KLC. By combining the results of a) and b), we can conclude that minimizing the false alarm probability and maximizing the detection probability is equivalent to maximizing the KLC.
Fig. 10. Empirical data distribution of the GKLD in the faulty case f(GKLD/H1 ).
5.1.2. Threshold optimization using KLC Finding the threshold that maximizes KLC is done by setting the derivative of KLC to 0: ∂∂KLC | = 0 where th denotes the th th=λ threshold. ∂P From the well-known relation ∂ PD = = the likelihood ratio, F
and setting Eq. (46) to zero, the likelihood ratio obtained from this optimization problem is as follows: u0
≶
u1
P0 A0 − A1 1 − P0 B1 − B0
with:
1 − P βi 0 log 2 P0 (αi + βi ) αi P0 Bi = log 2 1 − P0 α + β ( i i)
Ai =
i = 0, 1
(49)
αi βi βi αi (50)
Fig. 11. Optimal threshold value λ using the Kullback–Leibler Criterion for P0 = 0.7.
70
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
Fig. 12. The (a) KLC0 and (b) KLC1 for the liberal and conservative thresholds respectively.
Fig. 14. The KLij residuals test for the exclusion of faults.
Fig. 13. ROC curve of the GKLD test with different selected thresholds.
In Bayesian decision theory, the choice of threshold is based on minimizing the average costs of the decisions made by the detector. The corresponding LRT obtained from this optimization is given below: u0
≶
u1
P0 C10 − C00 1 − P0 C01 − C11
(51)
Where Cij is the cost of choosing hypothesis Hi when Hj is true; these costs are supposed to be known. (More details about the Bayesian detection could be found in [58]). However, the optimization problem using the KLC leads to costs Ai and Bi expressed in function of the posterior probability. Note that the mutual information criterion mentioned in [44] can be viewed as the expectation of the KLD between the prior and posterior distributions. On the other side, the KLC can be written as the summation of two terms:
KLC = K LC0 + K LC1 KLC0 = KL( p(H/u0 )|| p(H )) KLC1 = KL( p(H/u1 )|| p(H ))
(52)
With similar demonstration as the one presented in paragraph 5.1.1, the optimization of the KLC0 leads to a threshold denoted thKLC0 . This threshold corresponds to the value that maximizes the information gain given by the decision u0 . Therefore, select u0 with
a confidence that the hypothesis is H0 corresponds to a maximal value of KLC0 . In this case, the detector indicates a default whenever there may be one; thKLC0 is a liberal threshold. Likewise, select u1 with a confidence that the hypothesis is H1 corresponds to a maximal value of KLC1 . The corresponding threshold is thKLC1 and it is a conservative one; in other words, the detector indicates a default when it is almost certain that there are one [59] (Fig. 3). Depending on the application, the user may prefer a liberal threshold or a conservative threshold or a threshold varying with a system parameter. The value of the threshold that maximizes the KLC or maximizes the summation KLC0 + KLC1 may vary from t hKLC0 to thKLC1 and is influenced by the value of the prior probability. For a large value of P0 (the probability of the hypothesis H0 ), the threshold will take the value t hKLC1 or it approaches t hKLC1 since KLC1 KLC0 . Because, in this case, the decision u1 (if H1 is correct) provides the maximum information gain and the decision u0 provides a limited amount of information. Consequently, the choice of this probability should be suitable to the application and to the frequency of occurrence of defects. In this work, taking into account the historical system behavior, we propose to derive the prior probability using the MLE [60].
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
71
Fig. 15. The KLij residuals tests with the corresponding thresholds values.
Fig. 16. Data distribution of the KL21 in the (a) non-faulty and (b) faulty cases.
Suppose that we have a random samples of chosen hypotheses: h = (h1 , h2 , . . . , hn ) where: hi = 0: if the hypothesis H0 is selected (no faults). The associated probability is P0 hi = 1: if the hypothesis H1 is selected (faults). The associated probability is 1- P0 In this work, the sample of hypotheses at instant k =n + 1 is obtained from the decisions made by the detector from k = 1 to n. Note, that we distinguish between the detector associated to the GKLD test and those associated to the KLij tests. Therefore, we suppose that Hi ∼ Bernouilli (1 − P0 ) which leads to the pdf over an observation hi as in Eq. (53):
f hi ; 1 − P0 = (1 − P0 )h P0 1−h i
i
(53)
The likelihood function L(P0 ) is by definition:
L(P0 ) =
n #
$n
(1 − P0 )h P0 1−h = (1 − P0 ) i
i
i=1
hi
P0 n−
$n
i=1
hi
(54)
i=1
In this work, we are interested by estimating P0 in such way that the observed sample h becomes more likely. So, we need to find the P0 that maximizes L(P0 ) which is equivalent to maximizing log (L(P0 )). Therefore the derivative of log (L(P0 )) with respect to P0 is calculated. After a simple development, we find:
$n Pˆ0 = 1 −
i=1
n
hi
(55)
This result will be applied to the GKLD and to the KLij tests.
72
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
Fig. 17. Optimal threshold value using the Kullback–Leibler Criterion for P0 = 0.7 applied on KL21 dataset.
5.2. Application to the residual test
Fig. 18. Trajectories after the FDE step (blue) compared to the reference trajectories (red). (For interpretation of the references to colour in this figure legend, the reader is referred to the web version of this article.) Table 1 The signature matrix.
Define the pdf of the GKLD under Hi as f(GKLD/Hi ): i = 0,1. Then the detection problem is reduced to:
H0 : GKLD ∼ f (GKLD/H0 ) H1 : GKLD ∼ f (GKLD/H1 )
(56)
Fig. 4 presents an illustrative example of these distributions where:
PF =
λ
PD =
∞
∞
λ
f (GKLD/H0 ) d (GKLD ) f (GKLD/H1 ) d (GKLD )
(57)
The corresponding LRT obtained from the optimization using the KLC is given below:
f (GKLD/H1 ) u0 P0 A0 − A1 ≶ f (GKLD/H0 ) u1 1 − P0 B1 − B0
(58)
Likewise, the decision criterion can be expressed as function of the observation GKLD: u0
GKLD ≶ u1
λ
= function of (Ai , Bi, P0, parameter of the pdf of the GKLD )
(59)
Odometer1 Odometer2 Odometer3 Kinect 1 Kinect 2 Kinect 3
KL21
KL12
KL31
KL13
KL32
KL23
1 1 0 1 0 0
1 1 0 0 1 0
1 0 1 1 0 0
1 0 1 0 0 1
0 1 1 0 1 0
0 1 1 0 0 1
After taking into account the values of the false alarm and detection probabilities that maximize the KLC, the optimal threshold on the GKLD (λ) is obtained. Fig. 5 gives the algorithm of the threshold optimization. In the fault isolation part, a similar study is done in order to set the optimal threshold to the KLij by observing their distributions in the faulty and non-faulty cases. Given their conception, sensitivities of KLij to certain errors sensors can be noticed. The goal here is to decide optimally which of the possible binary values 0 (no fault) or 1 (fault) to set for each KLij in order to be compared with a signature matrix [61]. This matrix links the set of residuals to the potential sensors errors of the group of robots (Table 1). The number of the calculated KLij (the number of column in the signature matrix) varies in function of the number of observations and robots.
Fig. 19. The position errors along the (a) x-axis and the (b) y-axis after the FDE step.
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
Likewise, for the number of the supervised sensors (number of line in the signature matrix), it may vary depending on the type and number of embedded sensors on the robots. Consequently, we can deduce that the size of the signature matrix adapts to the chosen application. The signature matrix given in Table 1 corresponds to the simple case of study presented in Section 6 where all robots observe others.
6. Application to the multi-robot collaborative localization The validation of the proposed framework is applied to a multirobot system moving in indoor environment (Fig. 6). The robots are equipped with wheel encoders, gyroscope, Kinect and a 360degree laser scanner (RPLIDAR). The robots were controlled to follow given reference trajectories, presented in red in Fig. 7. The estimated trajectories of the three robots in the case of defaults in the odometers data and the Kinects measurements are presented in blue in Fig. 7. One can remark the divergence in the estimated trajectories (blue) compared to the references (red). Notice that the estimated trajectories are obtained from the fusion of the faulty Kinect or encoders measurements with the other sensors data using the EIF. Fig. 8 gives the GKLD used for the faults detection. It presents many jumps indicating a drift between the predicted data distribution obtained from the encoders measurements and the corrected one obtained after the incorporation of the relative observations. This statistical test should be compared to a threshold value. For the threshold calculation, the data distributions of the GKLD in the faulty and non-faulty cases should be determined in order to find the false alarm and detection probabilities that maximize the KLC. The data distribution of the GKLD in the non-faulty case is shown in Fig. 9: the blue histogram represents the empirical data obtained from real experimental results using the different incorporated sensors on robots; the red curve is the theoretical curve fitting as calculated in Eq. (35). Fig. 10 presents the experimental data distribution of the GKLD in the faulty case after taking into account the different types of faults that can occur on the Kinects and the encoders measurements. Therefore, the threshold is the GKLD value that maximizes the KLC and is obtained for example at the value 0.271 when the prior probability of the hypothesis H0 is P0 = 0.7 (Fig. 11). Note that the prior probability is obtained, at each sampling instant k, from the MLE and using the decisions made from instant 1 to k-1 as the observed sample of hypotheses. Therefore, the P0 value adapts with the system historical behavior leading to a variable threshold. The conservative and liberal thresholds are obtained from the optimization of KLC0 and KLC1 as shown in Fig. 12. [thKLC0 , thKLC1 ] form an envelope in which the threshold varies (Fig. 8). The performance of the detection framework using the GKLD test is shown in Fig. 13. The ROC curve lies above the diagonal and it’s near the point (PF = 0, PD = 1) reflecting the goodness of the proposed test in discriminating between the faulty and non-faulty behaviors. For different priori probability of the hypothesis, the optimal thresholds obtained from the maximization of the KLC are shown on the ROC curve. The one which maximizes the distance from the ROC to the diagonal line (Youden index) corresponds to the green marker. For P0 → 1, the threshold λ tends to thKLC1 , leading to a low false alarm and detection probabilities (for more details refer to Section 5.1.2). Similarly, for P0 → 0, λ approaches thKLC0 .
73
After the detection step, the faulty sensors should be excluded from the fusion procedure. Therefore, the set of residuals KLij must be found (Fig. 14). For the threshold calculation, a similar study as the one presented for the GKLD test is done for each KLij by finding their data distributions in the faulty and non-faulty cases. Therefore, f(KLij /H0 ) and f(KLij /H1 ) are obtained. After estimating the prior probability at instant k using the MLE, the maximization of the KLC applied to each of the KLij leads to an optimal threshold value (Fig. 15). For example, the data distribution of the KL21 in the faulty and non-faulty cases are shown in Fig. 16. In Fig. 17, for P0 = 0.7 the KLC is applied to the KL21 pdfs in order to find the optimal threshold. The value of the prior probability is estimated using the historical behavior of the detector KL21 and the MLE. Finally, when a fault is detected using the GKLD, the faulty sensors could be excluded from the fusion procedure after comparison with the signature matrix given in Table 1. At sampling instant k = 20 to 50, the KL21 and KL31 exceed the thresholds values, indicating that the Kinect 1 is faulty (Fig. 15). Similar reasoning enables to conclude that the Kinect 3 and 2 are faulty from k = 82 to 91 and k = 103 to 110 respectively. The odometer 3 is faulty or the robot presents a slippage at instants k = 190 to 192… Fig. 18 presents the trajectories of the three robots after the exclusion of the faulty sensors from the fusion procedure. The estimated trajectories are roughly similar to the references. After the FDE step, in order to quantify the accuracy of the proposed localization algorithm, the errors along the x-axis and the y-axis are shown in Fig. 19. 7. Conclusion In this paper, a multi-sensor fusion framework with faults detection and exclusion is developed. The method is applied to a CL of a multi-robot system. The FDE step is formulated using the KLD between the predicted and the corrected distributions of the IF. The residual tests based on the KLD present many advantages as they take into account the Burg matrix divergence between covariance matrices in addition to the Mahalanobis distance. The GKLD test detects the presence of faulty sensors in the robots team. A bank of EIF is used to generate the KLij set in order to exclude the faulty sensors after comparison with a signature matrix. Optimal thresholding method using the KLC is developed in order to replace previous approaches that set heuristically the false alarm probability. The optimal threshold value is influenced by the prior probability of the hypothesis estimated using the MLE and the historical system behavior. An interval of threshold could be constructed where the threshold may vary from a liberal to a conservative one. Experimental study of the proposed framework is done on real data obtained from sensors integrated on a group of robots. The framework is able to localize accurately the group after excluding the faulty sensors. In future work, a 3D model of the indoor environment will be integrated in the data fusion algorithm in order to increase the localization accuracy and to ensure the required redundancy for the faults exclusion. Acknowledgment This work has been supported by the Regional project SUCRé (Sûreté de fonctionnement et résilience pour la gestion et le contrôle coopératifs des systèmes sociotechniques: Coopération Homme(s)-Robot(s) en milieu hostile) of the Hauts-de-France region.
74
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
Appendix A Consider for example the first time when robot 1 detects robot 2:
1 21 γ T θk/k −1 hk 1 T ˜ 21 = γ θk/k−1 h k
Hk21 =
I3∗3
03∗3
(60)
where:
˜ ji h k
=
−1 0 0
y j − yi −x j + xi −1
0 −1 0
(61) k/k−1
i In the rest of the paper, γ (θk/k ) is denoted by γ (θki ). −1
T
The information matrix is updated according to the equation: Yk/k = Yk/k−1 + H 21 (R21 )−1 Hk21 k k
⎛ ⎝
T
−1 Hk21 (R21 Hk21 = k )
⎞
˜ 21 T h 1 21 −1 T 1 21 k ˜ I3∗3 ⎠γ θk (Rk ) γ θk h k 03∗3
I3∗3
03∗3
(62)
Notice that Yk/k−1 is block diagonal since it’s the first update in the multi-robot team. Therefore:
⎛
1 21 −1 T 1 21 θk (Rk ) γ θk h˜ k 1 21 −1 T 1 21 γ θk (Rk ) γ θk h˜ k
T −1 (h˜ 21 ) γ θk1 (R21 ) γ T θk1 k k −1 22 Yk/k + γ θk1 (R21 ) γ T θk1 −1 k
11 ˜ 21 )T γ Yk/k + (h −1 k
⎜ ⎝
Yk/k = ⎜
03∗3
⎞
03∗3
⎟ ⎟ ⎠
03∗3
(63)
33 Yk/k −1
03∗3
The covariance matrix is the inverse of the information matrix and gets the form:
⎛ Pk/k = ⎝
( ) γ (θk1 )(R21k ) γ T (θk1 ) h˜ 21k −1 γ (θk1 )(R21 γ T (θk1 ) h˜ 21 k ) k
11 ˜ 21 Yk/k + h −1 k
(h˜ 21k ) γ (θk1 )(R21k ) γ T (θk1 ) −1 22 Yk/k +γ (θk1 ) (R21 γ T (θk1 ) −1 k )
−1
T
−1
T
⎞
−1 06∗3
⎠
(64)
33 Pk/k −1
03∗6 The information vector is updated according to the equation:
−1 21 21 yk/k = yk/k−1 + (Hk21 )T (R21 k ) [ Zk − Z k estimate
+ Hk21 Xk/k−1 ]
(65)
And:
yk/k =
y1 y2 y3
k/k k/k k/k
⎛
y1
⎜ ⎝
1 21 −1 21 21 1 2 ⎞ ˜ X θk ( R k ) Zk − Z 21 + γ T θk1 h + γ T θk1 Xk/k k estimate k k/k−1 −1 1 21 −1 21 1 21 1 1 2 ⎟ 21 T T ˜ X y2 k/k−1 + γ θk (Rk ) Zk − Z k estimate + γ θk h + γ θk Xk/k−1 ⎟ k k/k−1 ⎠
k/k−1
=⎜
˜ 21 )T γ + (h k
y3
(66)
k/k−1
where: 21 Zestimate
−I3∗3 = γ θk1 T
I3∗3
03∗3
1 X X2 X3
(67) k/k−1
The pose estimate is then obtained from yk/k :
⎛
−1 Xk/k = Yk/k
( ) ⎜ −1 ) γ T (θk1 ) yk/k = Pk/k yk/k =⎝ γ (θk1 )(R21 k
( )
11 ˜ 21 )T γ θ 1 (R21 )−1 γ T θ 1 Yk/k + (h −1 k k k k
˜ 21 h k
˜ 21 h k
(h˜ 21 ) γ (θk1 )(R21 ) γ T (θk1 ) k k T
−1
( )
−1
22 Yk/k +γ θk1 (R21 ) −1 k
33 Pk/k y3 −1
γ T (θk1 )
−1
y 1
k/k
y2
k/k
⎞ ⎟ ⎠
(68)
k/k−1
Notice, from Eq. (64) and (68), that the robot 3 is not influenced by the observation between robot 1 and 2: 33 33 Pk/k = Pk/k −1 3 3 Xk/k = Xk/k −1
(69)
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
75
However, after the first update, where different robots detect each other, the cross correlation terms in the covariance matrix are not null. Consider now for example the case when robot 2 detects robot 3:
γ T θk2 h32 k 2 T = γ θk 03∗3
Hk32 =
T
˜ 32 h k −1
Yk/k = Yk/k−1 + H 32 (R32 k k )
⎛
11 Yk/k −1
=
⎜ 21 ⎜Y ⎝ k/k−1 31 Yk/k −1
I3∗3
(70)
Hk32 12 Yk/k −1
−1 γ θk2 (R32 ) γ T θk2 h˜ 32 k k 2 32 −1 T 2 32 32 ˜ Yk/k−1 + γ θk (Rk ) γ θk hk T
22 ˜ 32 ) Yk/k + (h −1 k
13 Yk/k −1
⎞
⎟ −1 γ θk2 (R32 ) γ T θk2 ⎟ k ⎠ 2 32 −1 T 2 33 Yk/k−1 + γ θk (Rk ) γ θk T
23 ˜ 32 ) Yk/k + (h −1 k
−1 Pk/k = Yk/k
(71)
(72)
And the information vector gets the form:
⎞ 32 2 3 ⎟ ⎜ ˜ 32 )T γ θ 2 (R32 )−1 Z 32 − Z 32 ˜ X + γ T θk2 h + γ T θk2 Xk/k yk/k = ⎝y2 k/k−1 + (h k k/k−1 k k k k estimate k −1 ⎠ 2 32 −1 32 2 32 2 2 3 32 T T ˜ X y3 k/k−1 + γ θk (Rk ) Zk − Z k estimate + γ θk h + γ θk Xk/k−1 k/k−1 k ⎛
y1
k/k−1
(73)
Therefore, the update of the poses estimate is obtained as follows: −1 Xk/k = Yk/k yk/k
(74)
Where the pose of robot 1 is: 1 11 Xk/k = Pk/k y1 13 +Pk/k
2 32 2 3 −1 32 T γ θk2 (R32 Zk − Z 32 θk h˜ k Xk/k−1 + γ T θk2 Xk/k k ) k estimate + γ −1 2 32 −1 32 T 2 T 3 y3 k/k−1 + γ θk (Rk ) Zk − Z 32 θk2 h˜ 32 θk2 Xk/k k estimate + γ k Xk/k−1 + γ −1
k/k−1
12 + Pk/k y2
T
k/k−1
˜ 32 ) + (h k
(75)
The same calculation is done for the poses estimate of robot 2 and 3. Notice that the observation between robot 2 and 3 influences the different poses estimation of the robots team. References [1] B. Khaleghi, A. Khamis, F.O. Karray, S.N. Razavi, Multisensor data fusion: a review of the state-of-the-art, Inf. Fusion 14 (2013) 28–44. [2] A. Frikha, H. Moalla, Analytic hierarchy process for multi-sensor data fusion based on belief function theory, Eur. J. Oper. Res. 241 (2015) 133–147. [3] S.J. Wellington, J.K. Atkinson, R.P. Sion, Sensor validation and fusion using the Nadaraya–Watson statistical estimator, in: Proc. Fifth Int. Conf. Inf. Fusion, vol.1, 2002, pp. 321–326. [4] M. Kumar, D.P. Garg, R.A. Zachery, A method for judicious fusion of inconsistent multiple sensor data, IEEE Sens. J. 7 (2007) 723–733. [5] L. Jiang, Sensor Fault Detection and Isolation using System Dynamics Identification Techniques, The University of Michigan, 2011. [6] H. Durrant-Whyte, Introduction to Decentralised Data Fusion, The University of Sydney, Sydney, Australia, 2004. [7] A.M. Martins, A.D. Neto, J.D. Melo, Comparison between Mahalanobis distance and Kullback–Leibler divergence in clustering analysis, WSEAS Trans. Syst. 3 (2004) 501–505. [8] F. Pérez-Cruz, Kullback–Leibler divergence estimation of continuous distributions, in: Inf. Theory 20 08 ISIT 20 08 IEEE Int. Symp. On, IEEE, 2008, pp. 1666–1670. [9] M. Bozorg, E.M. Nebot, H.F. Durrant-Whyte, A decentralised navigation architecture, in: 1998 IEEE Int. Conf. Robot. Autom. 1998 Proc., vol.4, 1998, pp. 3413–3418. [10] R. Sivalingam, D. Boley, V. Morellas, N. Papanikolopoulos, Tensor sparse coding for positive definite matrices, Pattern Anal. Mach. Intell. IEEE Trans. On 36 (2014) 592–605. [11] I.S. Dhillon, J.A. Tropp, Matrix nearness problems with Bregman divergences, SIAM J. Matrix Anal. Appl. 29 (2007) 1120–1146. [12] J. Dhillon, Differential entropic clustering of multivariate Gaussians, Adv. Neural Inf. Process. Syst. 19 (2007) 337. [13] S. Eguchi, J. Copas, Interpreting Kullback–Leibler divergence with the Neyman— Pearson lemma, J. Multivar. Anal. 97 (2006) 2034–2040. [14] J.-G. Wang, Test statistics in Kalman Filtering, Positioning (2008) 01. [15] S. Gamse, F. Nobakht-Ersi, M.A. Sharifi, Statistical process control of a Kalman filter model, Sensors 14 (2014) 18053–18074. [16] I. Hozo, B. Djulbegovic, S. Luan, A. Tsalatsanis, G. Gigerenzer, Towards theory integration: threshold model as a link between signal detection theory, fast-and-frugal trees and evidence accumulation theory, J. Eval. Clin. Pract. (2016).
[17] I.M. Rekleitis, G. Dudek, E.E. Milios, Multi-robot cooperative localization: a study of trade-offs between efficiency and accuracy, IEEERSJ Int. Conf. Intell. Robots Syst. vol.3 (2002) 2690–2695 2002. [18] S.I. Roumeliotis, G.A. Bekey, Distributed multirobot localization, IEEE Trans. Robot. Autom. 18 (2002) 781–795. [19] L.C. Carrillo-Arce, E.D. Nerurkar, J.L. Gordillo, S.I. Roumeliotis, Decentralized multi-robot cooperative localization using covariance intersection, in: Int. Conf. Intell. Robots Syst. IROS 2013 IEEERSJ, IEEE, 2013, pp. 1412–1417. [20] A. Martinelli, F. Pont, R. Siegwart, Multi-robot localization using relative observations, in: Proc. 2005 IEEE Int. Conf. Robot. Autom. 2005 ICRA 2005, 2005, pp. 2797–2802. [21] A. Howard, M.J. Matark, G. Sukhatme, Localization for mobile robot teams using maximum likelihood estimation, in: IEEERSJ Int. Conf. Intell. Robots Syst, vol.1, 2002, pp. 434–439. [22] R.A. Carrasco, F. Núñez, A. Cipriano, Fault detection and isolation in cooperative mobile robots using multilayer architecture and dynamic observers, Robotica 29 (2011) 555–562. [23] P.J. Escamilla-Ambrosio, N. Mort, A hybrid Kalman filter-fuzzy logic multisensor data fusion architecture with fault tolerant characteristics, in: Int. Conf. Artif. Intell, 2001, pp. 361–367. [24] P.H. Ibarguengoytia, L.E. Sucar, S. Vadera, Real time intelligent sensor validation, IEEE Trans. Power Syst 16 (2001) 770–775. [25] T. Wei, Y. Huang, P. Chen, Sensor validation for flight control by particle filtering, in: Signal Process. Conf. 2005 13th Eur., 2005, pp. 1–4. [26] D. Del Gobbo, M. Napolitano, P. Famouri, M. Innocenti, Experimental application of extended Kalman filtering for sensor validation, IEEE Trans. Control Syst. Technol 9 (2001) 376–380. [27] R. Grabowski, L.E. Navarro-Serment, C.J. Paredis, P.K. Khosla, Heterogeneous teams of modular robots for mapping and exploration, Auton. Robots. 8 (20 0 0) 293–308. [28] D. Foxy, W. Burgardz, H. Kruppayy, S. Thruny, Collaborative multi-robot localization, KI-99 Adv. Artif. Intell. 23rd Annu. Ger. Coference Sic Artif. Intell., 255, 1999. [29] H. Li, F. Nashashibi, Cooperative multi-vehicle localization using split covariance intersection filter, in: 2012 IEEE Intell. Veh. Symp. IV, 2012, pp. 211–216. [30] S. Panzieri, F. Pascucci, R. Setola, Multirobot localisation using interlaced extended Kalman filter, in: 2006 IEEERSJ Int. Conf. Intell. Robots Syst., 2006, pp. 2816–2821.
76
J. Al Hage et al. / Information Fusion 37 (2017) 61–76
[31] E.D. Nerurkar, S.I. Roumeliotis, A. Martinelli, Distributed maximum a posteriori estimation for multi-robot cooperative localization, in: IEEE Int. Conf. Robot. Autom., 2009, pp. 1402–1409. [32] P. Sundvall, P. Jensfelt, Fault detection for mobile robots using redundant positioning systems, in: Proc. 2006 IEEE Int. Conf. Robot. Autom. 2006 ICRA 2006, 2006, pp. 3781–3786. [33] E. Guan, J. Fei, G. Pan, Z. Fu, W. Yan, Y. Zhao, Fault self-diagnosis for modular robotic systems using M-lattice modules, Int. J. Adv. Robot. Syst. (2015) 12. [34] A.L. Christensen, R. O’Grady, M. Dorigo, From fireflies to fault-tolerant swarms of robots, IEEE Trans. Evol. Comput 13 (2009) 754–766. [35] X. Li, L.E. Parker, Sensor analysis for fault detection in tightly-coupled multi-robot team tasks, in: 2007 IEEE Int. Conf. Robot. Autom., 2007, pp. 3269–3276. [36] X. Li, L.E. Parker, Distributed sensor analysis for fault detection in tightly-coupled multi-robot team tasks, in: IEEE Int. Conf. Robot. Autom. 2009 ICRA 09, 2009, pp. 3103–3110. [37] R. Tinós, L.E. Navarro-Serment, C.J. Paredis, Fault tolerant localization for teams of distributed robots, in: Proc. Int. Conf. Intell. Robots Syst. 2001 IEEERSJ, IEEE, 2001, pp. 1061–1066. [38] J. Zeng, U. Kruger, J. Geluk, X. Wang, L. Xie, Detecting abnormal situations using the Kullback–Leibler divergence, Automatica 50 (2014) 2777–2786. [39] Y. Morales, E. Takeuchi, T. Tsubouchi, Vehicle localization in outdoor woodland environments with sensor fault detection, in: IEEE Int. Conf. Robot. Autom. 2008 ICRA 2008, IEEE, 2008, pp. 449–454. [40] Z. Qiang, Z. Xiaolin, C. Xiaoming, Research on RAIM algorithm under the assumption of simultaneous multiple satellites failure, in: Eighth ACIS Int. Conf. Softw. Eng. Artif. Intell. Netw. ParallelDistributed Comput. 2007 SNPD 2007, 2007, pp. 719–724. [41] A. Hero, Signal detection and classification, 1997. [42] J. Harmouche, C. Delpha, D. Diallo, Incipient fault detection and diagnosis based on Kullback–Leibler divergence using principal component analysis: Part II, Signal Process 109 (2015) 334–344. [43] T. Kanungo, R.M. Haralick, Receiver operating characteristic curves and optimal Bayesian operating points, in: Int. Conf. Image Process. 1995 Proc., vol.3, 1995, pp. 256–259. [44] I.Y. Hoballah, P.K. Varshney, An information theoretic approach to the distributed detection problem, IEEE Trans. Inf. Theory 35 (1989) 988–994.
[45] D. Pomorski, Entropy-based optimisation for binary detection networks, in: Proc. Third Int. Conf. Inf. Fusion 20 0 0 FUSION 20 0 0, 2, 20 0 0 p. THC4/3-THC410. [46] R.J. Irwin, T.C. Irwin, A principled approach to setting optimal diagnostic thresholds: where ROC and indifference curves meet, Eur. J. Intern. Med. 22 (2011) 230–234. [47] P. Bonnifait, Localisation précise en position et attitude des robots mobiles d’extérieur à évolutions lentes, Ecole centrale de Nantes (1997). [48] S.I. Roumeliotis, I.M. Rekleitis, Analysis of multirobot localization uncertainty propagation, in: Int. Conf. Intell. Robots Syst. IROS 2003 IEEERSJ, IEEE, 2003, pp. 1763–1770. [49] M. Lexa, Useful Facts about the Kullback–Leibler Discrimination Distance, Rice University, Houston, 2004. [50] M.N. Do, Fast approximation of Kullback–Leibler distance for dependence trees and hidden Markov models, IEEE Signal Process. Lett. 10 (2003) 115–118. [51] J. Al Hage, N.A. Tmazirte, M. El badaoui el najjar, D. pomorski, Fault detection and exclusion method for a tightly coupled localization system, 17th Int. Conf. Adv. Robot. ICAR 2015, 2015. [52] J. Al Hage, N. AitTmazirte, M. E.El Najjar, D. Pomorski, Fault tolerant fusion approach based on information theory applied on GNSS localization, in: 2015 18th Int. Conf. Inf. Fusion Fusion, 2015, pp. 696–702. [53] D.I. Belov, R.D. Armstrong, Distributions of the Kullback–Leibler divergence with applications, Br. J. Math. Stat. Psychol. 64 (2011) 291–309. [54] A.C. Rencher, Methods of Multivariate Analysis, John Wiley & Sons, 2003. [55] R.P. McDonald, Testing pattern hypotheses for covariance matrices, Psychometrika. 39 (1974) 189–201. [56] P. Baldi, L. Itti, Of bits and wows: a Bayesian theory of surprise with applications to attention, Neural Netw. 23 (2010) 649–666. [57] L. Itti, P.F. Baldi, Bayesian surprise attracts human attention, Adv. Neural Inf. Process. Syst. (2005) 547–554. [58] M.A. Figueiredo, Lecture notes on Bayesian estimation and classification, Inst. Telecomunicacoes-Inst. Super. Tec. (2004). [59] H. Abdi, Signal detection theory (SDT), in: Encyclopedia of Measurement and Statistics, Thousand Oaks (CA), Sage, 2007, pp. 886–889. [60] F.W. Scholz, Maximum likelihood estimation, Encyclopedia of Statistical Sciences, John Wiley & Sons, Inc., 2004. [61] J. Gertler, Analytical redundancy methods in fault detection and isolation, in: Prepr. IFACIMACS Symp. Fault Detect. Superv. Saf. Tech. Process. SAFEPROCESS’91, 1991, pp. 9–21.
