Multi-sensor data fusion based on Information Theory. Application to ...
Multi-sensor data fusion based on Information Theory. Application to GNSS positionning and integrity monitoring Nourdine Aït Tmazirte, Maan E. El Najjar, Cherif Smaili and Denis Pomorski LAGIS UMR 8219 CNRS/Université-Lille1 Avenue Paul Langevin 59655 Villeneuve d’Ascq, France [email protected] through statistical approach [6]. In this residual test scheme, generally, FD is conducted by examining the probabilistic property of the estimated state itself [5], yet a precise reference system unaffected by failures is required. The implementation of this kind of architecture requires a thresholding process in order to pick out measurements errors. This kind of process could easily detect sporadic measurements errors but hardly detect the gradually increasing error of measurements. In addition, thresholds are fixed classically with a heuristic manner.
Abstract — Integrity monitoring is considered now as an
important part of a vehicle navigation system. Localisation sensors faults due to systematic malfunctioning require integrity reinforcement of multi-sensors fusion method through systematic analysis and reconfiguration method in order to exclude the erroneous information from the fusion procedure. In this paper, we propose a method to detect faults of the GPS signals by using a distributed information filter with a probability test. In order to detect faults, consistency is examined through a log likelihood ratio of the information innovation of each satellite using mutual information concept. Through GPS measurements and the application of the autonomous integrity monitoring system, the current study illustrates the performance of the proposed fault detection algorithm and the pertinence of the reconfiguration of the multisensors data fusion.
Recently, the information filter (IF), which is the informational form of the KALMAN filter (KF), has proved to be attractive for multi-sensors fusion, like in [7],[8] or [9]. The IF uses an information matrix and an information vector to represent the co-variance matrix and the state vector usually used in a KF. This difference in representation makes the IF superior to the KF concerning multiple sensor fusion, as computations are simpler and no prior information of the system state is required [10] [11]. In addition, consistency test using log likelihood ratio (LLR), based on information matrix, permits to have a good balance between the detection probability and the false alarm probability.
Keywords:data fusion, Fault detection, information theory, localisation, GNSS, RAIM.
I.
INTRODUCTION
Autonomous navigation system requires a safety positioning system. When leading safety, positioning services not only need to provide an estimate of the vehicle location, but also uncertainty estimation. In practice, an upper bound on the positioning error, representing an integrity risk, is required to determine if a positioning system can be used for a given task. At GNSS (Global Navigation Satellite System) receiver level, standard approaches introduce a Fault Detection & Exclusion (FDE) stage to monitor the integrity of the estimation of the position. This is known as Receiver Autonomous Integrity Monitoring (RAIM) [1] [2]. Numerous FD studies have been conducted with real time sensors measurements. When we do not dispose of a sensor model, which is always the case with localisation sensors, FDE methods are approached in a statistical manner. Classically, stochastic approaches are developed through KALMAN filters [3], Particle Filter [4] or interval analysis [5]. These procedures for FD introduce residual testing
With the help of information filtering, the main interest of this paper is to ensure the reliability of the GNSS positioning in a navigation system. In this paper, a new GNSS integrity monitoring method is presented by applying a distributed IF and a LLR test. Specifically, the presented (FDE) scheme takes advantage of the highly LLR change between the information of each satellite and the mutual information of a set of visible satellites by a GNSS (GPS) receiver. The performance of the distributed IF is notable after reconfiguration of the data fusion procedure. This paper is organized as follows: Section 2 briefly introduces the general principle of distributed IF. In section 3, a detailed GPS integrity monitoring algorithm that employs distributed IF and ratio test is illustrated. A result using real GPS measurement
743
data is presented in section 4, followed by the conclusion in section 5. II.
𝐼(𝑘) is the associated information matrix. The couple (𝑖 𝑘 , 𝐼(𝑘)) represents the Information Contribution (IC) of the observation.
INFORMATION THEORY AND FILTERING
In multiple sensor problems:
A. Information filter
zi k = Hi k . x k + vi k
An Extended KF can easily be expressed in another form, called Nonlinear Information Filter (NIF) or inverse covariance filter [10]. Instead of working with the estimation of states 𝑥(𝑖|𝑗) and the variance 𝑃(𝑖|𝑗) , the IF deals with the information state vector 𝑦(𝑖|𝑗) and information matrix 𝑌(𝑖|𝑗), where: y 𝑖 𝑗 = P −1 𝑖 𝑗 . x 𝑖 𝑗
(1)
𝑌(𝑖|𝑗) = 𝑃 −1 (𝑖|𝑗)
(2)
(9)
i = 1…N
the estimate does not represent a simple linear combination of contributions from individual sensors : (10) 𝑥 𝑘 𝑘 ≠𝑥 𝑘 𝑘−1 + 𝑁 𝑖=1 𝑊𝑖 𝑘 . 𝑧𝑖 𝑘 − 𝐻𝑖 𝑘 . 𝑥 𝑘 𝑘 − 1 (with 𝑊𝑖 (𝑘) independent gain matrices). The innovation generated from each sensor is correlated. Indeed, they share common information through the prediction 𝑥 (𝑘|𝑘 − 1).
The information matrix is closely associated with the Fisher information. The physical mean of the Fisher information is the surface of a bounding region containing probability mass. It measures the compactness of a density function.
In information form, estimates can be constructed from linear combinations of observation information.
(3)
It is because information terms 𝑖𝑖 (𝑘) from each sensor are assumed to be uncorrelated. Now it is straightforward to evaluate the contribution of each sensor (unlike for a KF). Each sensor node simply generates the information terms 𝑖𝑖 (𝑘). These are summed at the update step of the NIF to produce a global information estimate.
𝑥 𝑘 = 𝐹 𝑘 .𝑥 𝑘 − 1 + 𝑤 𝑘
𝑦 𝑘 𝑘 = 𝑦 𝑘 𝑘−1 +
Where 𝑥(𝑘) is the state vector, 𝐹(𝑘) the state transition matrix and 𝑤(𝑘) the process noise.
𝑁 𝑖=1 𝑖𝑖
𝑘
(11)
The observation is also modeled: 𝑧 𝑘 = 𝐻 𝑘 . 𝑥 𝑘 + 𝑣(𝑘)
B. Mutual Information
(4)
Where 𝑧(𝑘) is the observation vector, 𝐻 𝑘 the observation matrix and 𝑣(𝑘) is a white noise.
To evaluate the IC of each sensor, a well known concept of Information theory can be used: the Mutual Information (MI). The MI 𝐼(𝑥, 𝑧) is an a priori measure of the information which will be gained by x with a set of observations z.
The update stage of the NIF is written :
𝐼(𝑥, 𝑧) is defined in [12] as:
𝑦 𝑘|𝑘 = 𝑦 𝑘|𝑘 − 1 + 𝑖 𝑘 𝑌 𝑘|𝑘 = 𝑌 𝑘|𝑘 − 1 + 𝐼 𝑘
(5) 𝐼 𝑥, 𝑧 = −𝐸 ln
(6)
𝑃 𝑥, 𝑧 𝑃 𝑥 .𝑃 𝑧 = −𝐸 ln
Where 𝑖 𝑘 = 𝐻 𝑇 𝑘 . 𝑅 −1 𝑘 . 𝑧(𝑘)
= −𝐸 ln
𝑃 𝑥𝑧 𝑃 𝑥
𝑃 𝑧𝑥 𝑃 𝑧
(12)
(7) In term of entropies, the MI is written:
𝑖(𝑘) is a vector containing the contribution in term of information from an observation 𝑧(𝑘). 𝐼 𝑘 = 𝐻𝑇 𝑘 . 𝑅−1 𝑘 . 𝐻𝑇 𝑘
𝐼 𝑥, 𝑧 = 𝐻 𝑥 + 𝐻 𝑧 − 𝐻 𝑥, 𝑧 = 𝐻 𝑥 − 𝐻 𝑥 𝑧
(8)
(13)
With 𝐻(𝑥), 𝐻(𝑧) the respective entropies of 𝑥 and 𝑧. 𝐻 𝑥 𝑧 and 𝐻 𝑧 𝑥 the conditional entropies.
744
III.
FAULTY
As well detailed in [13], the process model depends on the dynamical characteristics of the vehicle. The system is assumed to evolve according to the equation:
DETECTION AND EXCLUSION : APPLICATION TO GPS INTEGRITY MONITORING
GNSS integrity monitoring or Receiver Autonomous Integrity Monitoring (RAIM) is a technology developed to control the integrity of global navigation satellite system signals in a receiver system. During a long time, integrity was exclusively used in aviation or marine navigation. The integrity represents the ability to associate to a position a reliable indication of trust.
𝑥 𝑘 + 1 = 𝐴. 𝑥 𝑘 + 𝑤 𝑘 + 1
(14)
Where the state vector is composed of the eight following variables: 𝑥 𝑘 = 𝑋 𝑘 , 𝑋 𝑘 , 𝑌 𝑘 , 𝑌 𝑘 , 𝑍 𝑘 , 𝑍 𝑘 , 𝑐𝜕𝑡 𝑘 , 𝑐𝜕𝑡 𝑘
With the development of different safety-critical urban applications, the notion of integrity became an important field of research, in particular in urban canyon, where satellites signals are subject to hostile environment.
(15)
[𝑋, 𝑌, 𝑍] representing the receiver position,[𝑋, 𝑌, 𝑍]the velocity in Earth Centered Earth Fixed (ECEF) frame, 𝜕𝑡 the clock range and 𝜕𝑡 the clock drift.
Usually, RAIM algorithms use a comparison of the result of a main equation system and subsystems results [4], we propose to use the information theory to quantify the information contribution of each satellite (Figure 1). In other words, we propose to detect and to exclude faulty satellite upstream, meaning before the equation system resolution.
The state transition matrix is given by: 1 0 0 0 0 0 0 0
𝐴=
𝑇 1 0 0 0 0 0 0
0 0 1 0 0 0 0 0
0 0 𝑇 1 0 0 0 0
0 0 0 0 1 0 0 0
0 0 0 0 𝑇 1 0 0
0 0 0 0 0 0 1 0
0 0 0 0 0 0 𝑇 1
(16)
Where 𝑇 is the sample period (in our case 1 second). The measurement noise 𝑤 . is assumed to be time invariant: 𝑤 . = 𝑇 2 𝜍𝑥 2
𝑇 2 𝜍𝑦 2
2
, 𝑇𝜍𝑥 2 ,
2
, 𝑐𝜍𝑔 2 𝑇
𝑐𝜍𝑔 2 𝑇 2
2
, 𝑇𝜍𝑦 2 ,
𝑇 2 𝜍𝑧 2 2
, 𝑇𝜍𝑧 2 , 𝑐𝜍𝑓 2 𝑇 +
(17)
Where 𝜍𝑥 2 , 𝜍𝑦 2 , 𝜍𝑧 2 represent the process noise variances related to the states 𝑥, 𝑦 and 𝑧, 𝜍𝑓 and 𝜍𝑔 are the variances related to clock offset and velocity, and 𝑐 the speed of light . Making the assumption that 𝑤~𝑁 0, 𝑄 means that 𝑤 is assumed to be a white Gaussian noise with covariance 𝑄 :
Figure 1. Satelllite Information contribution for GNSS Monitoring
A. State space representaion for GNSS Positionning GNSS positioning with pseudo-range is a Time of Arrival method [5]. Pseudo-ranges are the distances between visible satellites and the receiver plus the unknown difference between the receiver clock and the GNSS time. Thus, GNSS positioning is a four dimensional problem: the 3D coordinates (𝑥𝑖 , 𝑦𝑖 , 𝑧𝑖 ) of the user and the clock offset 𝑑𝑡𝑖 are unknown.
𝑄=
745
0 0 0 0 0 𝛼𝜍𝑥 2 𝛽𝜍𝑥 2 0 0 0 2 2 0 0 0 0 𝛽𝜍𝑥 𝛾𝜍𝑥 2 2 0 0 𝛼𝜍 0 0 𝛽𝜍 𝑥 𝑥 0 0 2 0 0 2 0 0 0 0 𝛽𝜍𝑥 𝛾𝜍𝑥 2 2 0 0 𝛼𝜍 𝛽𝜍 𝑥 𝑥 0 0 0 0 0 0 0 0 𝛽𝜍𝑥 2 𝛾𝜍𝑥 2 0 0 2 + 𝛼𝜍 2 𝛽𝜍 2 𝛾𝜍 𝑓 𝑔 𝑔 0 0 0 0 0 0 2 2 𝛽𝜍 𝛾𝜍 0 0 0 0 𝑔 𝑔 0 0
(18)
…
𝜍1
With 𝑐2𝑇3 3
(19)
𝑐2 𝑇2 𝛽= 2
(20)
𝛼=
𝛾=
⋱ 𝑅(. ) =
0
(21)
𝑇
𝑥𝑖 − 𝑥 𝑆 2 + 𝑦𝑖 − 𝑦 𝑆 2 + 𝑧𝑖 − 𝑧 𝑆 𝑐. 𝑑𝑡𝑖 − 𝑑𝑡 𝑠 + 𝛿𝜌𝑖𝑆𝐼𝑜𝑛𝑜 + 𝛿𝜌𝑖𝑆𝑇𝑟𝑜𝑝𝑜
𝐻=
With 𝑗 ∇𝑥
2
𝐻 𝑋 =
+
𝑗
=
1 0 ∇𝑦 0 ∇1𝑧 0 ⋮ ⋮ ⋮ ⋮ ⋮ 𝑗 0 ∇𝑦 0 ∇𝑧𝑗 0 ⋮ ⋮ ⋮ ⋮ ⋮ 0 ∇𝑦𝑛 0 ∇𝑧𝑛 0
1 ⋮ 1 ⋮ 1
(24)
2
+ 𝑧 𝑆 − 𝑧𝑝𝑟
(25)
(26)
2
1 𝑃(𝑋) ln 2 𝑃(𝑋|𝑍)
(30)
(31)
A Global Information Mutual Information (GOMI) can be defined. 𝑃(𝑋) is the prediction covariance and 𝑃(𝑋|𝑍) is the updated covariance. The GOMI becomes, in information form: 1 𝑌(𝑘|𝑘) ln 2 𝑌(𝑘|𝑘 − 1)
1 𝑌 𝑘 𝑘−1 + 𝑁 𝑖=1 𝐼𝑖 (𝑘) 𝐼 𝑋, 𝑍 = ln 2 𝑌(𝑘|𝑘 − 1)
And + 𝑦 𝑆 − 𝑦𝑝𝑟
(29)
𝐼 𝑋, 𝑍 =
𝐼 𝑋, 𝑍 =
𝜕𝑅𝑗𝑠 𝑗 𝑠 ∇𝑧 = = −(𝑧 𝑆 − 𝑧𝑝𝑟 ) 𝑅𝑝𝑟 𝜕𝑧
2
𝑑 ln(2𝜋) 𝜀 + 𝑑. + ln 2 2 2
𝑑 ln 2𝜋 𝑃(𝑋) 𝑑 ln 2𝜋 + 𝑑. + ln − ( + 𝑑. 2 2 2 2 2 𝑃(𝑋|𝑍) + ln ) 2 It becomes a basic Log Likelihood Ratio :
(23)
𝜕𝑅𝑗𝑠 𝑠 = −(𝑦 𝑆 − 𝑦𝑝𝑟 ) 𝑅𝑝𝑟 𝜕𝑦
𝑥 𝑆 − 𝑥𝑝𝑟
𝜍𝑛
From Eq.13 & Eq. 29, the MI in our case is defined:
0 ⋮ 0 ⋮ 0
𝐼 𝑋, 𝑍 =
𝑠 𝑅𝑝𝑟 =
(28)
𝑁 𝑋 . ln 𝑁 𝑋 . 𝑑𝑋
(22)
𝜕𝑅𝑗𝑠 𝑠 = = −(𝑥 𝑆 − 𝑥𝑝𝑟 ) 𝑅𝑝𝑟 𝜕𝑥
∇𝑦 =
…
⋮ ⋱
As proved in [14] the entropy, for a multivariate Gaussian distribution (which is assumed to be the case in Eq. 14 for our study), goes as the log-determinant of the covariance. Precisely, the differential entropy of a d-dimensional random vector 𝑋 drawn from the Gaussian 𝑁(𝜇, 𝜀) is:
Being non-linear, the observation model is linearised around a predicted state [𝑋𝑝𝑟 ] to obtain an observation matrix: ∇1𝑥 ⋮ 𝑗 ∇𝑥 ⋮ ∇𝑥𝑛
𝜍𝑗
B. Mutual Information for FDE
The observation [𝑥𝑖 , 𝑦𝑖 , 𝑧𝑖 ] of each satellite 𝑖 , is calculated thanks to broadcasted ephemeris. The atmospheric errors are also modeled 𝛿𝜌𝑖𝑆𝐼𝑜𝑛𝑜 , 𝛿𝜌𝑖𝑆𝑇𝑟𝑜𝑝𝑜 . The pseudo-range can be modeled as follow: 𝑅𝑖𝑠 =
⋮
0
(32)
(33)
We also define the Partial Observation Mutual Information (POMI) which will take in account only one information term of sensor 𝑖.
(27)
Each satellite noise is assumed to be uncorrelated with all others. So, the noise described as follow 𝑣~𝑁 0, 𝑅 is simply represented by its diagonal time-invariant matrix R:
1 𝑌 𝑘 𝑘 − 1 + 𝐼𝑖 (𝑘) 𝐼𝑖 𝑋, 𝑍 = ln 2 𝑌(𝑘|𝑘 − 1)
(34)
The GOMI will prove to be an adapted residual for fault detection, whereas the POMI will isolate the faulty observation.
746
C. Principle of the FDE method
thresholding stage in order to detect the inconsistency of one or several satellites. In case of fault detection, a POMI (Eq. 34) is computed for each satellite. These different POMIs are used to isolate the faulty satellites measurements in order to be excluded from the correction step of the NIF algorithm.
The structure of the proposed GNSS integrity monitoring method is illustrated in figure 2 which correspond to a classical prediction/correction filtering using IF but including a FDE stage.
IV.
EXPERIMENTAL RESULTS
In order to test the performance of the RAIM developed approach, real data acquisition has been carried out with CyCab vehicle produced by Robosoft (www.robosoft.fr/) with several embedded sensors. In this work, measurements of GPS RTK Thales Sagitta 02 system and open GPS Septentrio Polarx2e@ (Figure 4) are used.
Figure 2. Classical GNSS Integrity Monitoring schema
In the proposed FDE approach, described in the following flowchart, in figure 3, only five measurements are needed for the integrity monitoring.
Figure 4.
Experimental vehicle.
The data acquisition has been carried out around LORIA Lab. During these experiments we remarked, as shown in figure 5, that when the positionning system uses all visible satellite without a FDE stage, one satellite introduces a bias in the positionning process. It seems that this satellite was in a bad geometric configuration in respect to the building in front (Bat C in the figure 4, 5 and 6). To compare, the test trajectory reference with centimetric accuracy (GPS RTK) is plotted in green in figure 6. This trajectory is about eighty meters length.
Figure 3. Information Theory based proposed method
With n measurements, n Information Contribution (IC1 … ICn) (Eq. 7 & Eq.8) are computed. A GOMI (Eq. 33) is then computed using the n IC. The GOMI permits to have an auto-
747
Figure 5.
Trajectory after fusion without FDE
Figure 7. Global Observation Mutual Information for Detection
The fault exclusion step is for each satellite. In fact, inconsistency of the faulty One can see the surges of blue. Figure 6.
realised using the POMI computed the POMI permits to identify the satellite like discribed in figure 8. POMI of satellite 6 represented in
Reference trajectory given by RTK GPS
In figure 7 and 8, the results of the detection and exclusion process are presented for the set of visible satellites during the test trajectory. GOMI concept, introduced in section 2 helps to auto-threshold information contribution of all satellites. After few steps of initialization, the contribution of each satellite goes to ―stabilize‖ around a value. Indeed, a satellite, if still visible, should not bring a contribution different to the information gives by the steps before. Figure 7, one can easily see the different surges of information contribution. That because a pseudo-range is generally overestimated when the corresponding satellite waves is subject to multi-path. Thus, the IC of the faulty satellite is also overvalued. This IC becomes inconsistent, and need to be excluded.
Figure 8.
Partials Observation Mutual Information for Exclusion
In figure 9, is shown the performance of the fusion with FDE after exclusion of satellite 6 from the fusion procedure. We note that the trajectory in figure 9 is close to the GPS RTK trajectory showed in figure 6.
In this test, we detected only one satellite real errors, but the proposed approach can be generalized for multiple faulty satellites.
748
introduced by each satellite observation compared to the expected information.
Figure 9.
The evaluation of FDE is conducted through simulation using real GPS measurements data. In the presented test trajectory, the measured data from GPS contain the well known wave’s multipath error illustrated with an abnormal bias. Based on real data, results demonstrate performance of the proposed approach in detecting GPS measurements faults and fusion method reconfiguration. In future work, our objective is to integrate dead reckoning sensors in order to develop a tightly coupled data fusion method. These sensors are known to introduce gradually increasing errors (drift). The LLR is well adapted to detect this kind of errors.
Trajectory after fusion with FDE
Finally, the figure 10 shows performance of the proposed approach with faulty satellite exclusion in blue. This figure shows the position computation in the ECEF frame for each axis (X,Y,Z). In red, are the position computation on ECEF frame without faulty satellite exclusion.
REFERENCES Erreur ! Source du renvoi introuvable. [1]
R. Grover Brown, Gerald Y. Chin, ―GPS RAIM: Calculation of Threshold and Protection Radius Using Chi-Square Methods-A Geometric Approach‖, in. Global Positioning System: Inst. Navigat., vol. V, pp. 155– 179, 1997 [2] T. Walter, P. Enge. ―Weighted RAIM for Precision Approach.‖ Proceedings of the ION-GPS 1995, Palm Springs, CA, 1995. [3] B. W. Parkinson, and P. Axelrad, ―Autonomous GPS integrity monitoring using the pseudorange residual‖, Navig., J. Inst. Navig., vol. 35, no. 2, pp. 255–274, 1988. [4] J. Ahn & al., ―GPS Integrity Monitoring Method Using Auxiliary Nonlinear Filters with Log Likelihood Ratio Test Approach‖, Journal of Electrical Engineering & Technology Vol. 6, No. 4, pp. 563~572, 2011. [5] V. Drevelle and Ph. Bonnifait ,―Global Positioning in Urban Areas with 3D Maps‖ Proceedings of the 2011 IEEE Intelligent Vehicles Symposium, Baden-Baden, Allemagne, pp. 764-769, 2011 [6] Y. C. Lee, ―Analysis or range and position comparison methods as a means to provide GPS integrity in the user receiver,‖ in Proceedings of the Annual Meeting of the Institute of Navigation, Seattle, WA, Jun. 1986, pp. 1–4, 1986. [7] B. Grocholsky, H. Durrant-Whyte, and P. Gibbens, ―An informationtheoretic approach to decentralized control of multiple autonomous flight vehicles,‖ Proc. SPIE: Sensor Fusion and Decentralized Control in Robotic Systems III, vol. 4196, pp. 348–359, Oct. 2000. [8] L. Wang, Q. Zhang , H. Zhu , and L. Shen, ―Adaptive consensus fusion estimation for msn with communication delays and switching network topologies‖. Decision and Control (CDC), 2010 49th IEEE Conference on, pages 2087 –2092,2010. [9] G. Liu, F. Wörgötter, and I. Markeli´c, ―Nonlinear estimation using central difference information filter‖. In IEEE Workshop on Statistical Signal Processing, pages 593–596, 2011. [10] S. Grime and H. F. Durrant-Whyte. ―Data Fusion in Decentralized Sensor Networks‖. Control Engineering Practice, 2:849-863, 1994. [11] D.-J. Lee, ―Nonlinear estimation and multiple sensor fusion using unscented information filtering‖. Signal Processing Letters, IEEE, 15:861 – 864, 2008. [12] T.M. Cover and J.A. Thomas, Elements of information theory. John Wiley & Sons, New York, NY, Inc., Chapter 9 Theroem 9.4.3, 1991. [13] S. Cooper and H. F. Durrant-Whyte. ―A Kalman filter model for GPS navigation of land vehicles‖. In: IEEE/RSJ/GI International Conference on Intelligent Robots and Systems, p. 157—163, 1994. [14] T.M. Cover and J.A. Thomas, Elements of information theory. John Wiley & Sons, New York, NY, Inc., Chapter 2 Definition 2.3, 1991.
Figure 10. Exclusion of a faulty satellite in GNSS positioning
V.
CONCLUSION AND FUTURES WORKS
This paper proposes a reliable multi sensor data fusion approach with integrity monitoring. This approach integrates Fault Detection stage by using distributed NIF and LLR test using mutual information. The proposed method makes it possible to detect fault without heuristic thresholing step. It permits an automatic reconfiguration of the fusion procedure in order to exclude the faulty measure. The proposed approach is applied for a GPS integrity monitoring. Measurements were pseudoranges from the GPS satellites. The basic concept is to use the Information Filter in a distributed architecture. The likelihood function is established and examined by integrating state estimate from distributed Information Filters. Thus, the LLR test is used in fault detection, which compares the information
749
Abstract — Integrity monitoring is considered now as an
important part of a vehicle navigation system. Localisation sensors faults due to systematic malfunctioning require integrity reinforcement of multi-sensors fusion method through systematic analysis and reconfiguration method in order to exclude the erroneous information from the fusion procedure. In this paper, we propose a method to detect faults of the GPS signals by using a distributed information filter with a probability test. In order to detect faults, consistency is examined through a log likelihood ratio of the information innovation of each satellite using mutual information concept. Through GPS measurements and the application of the autonomous integrity monitoring system, the current study illustrates the performance of the proposed fault detection algorithm and the pertinence of the reconfiguration of the multisensors data fusion.
Recently, the information filter (IF), which is the informational form of the KALMAN filter (KF), has proved to be attractive for multi-sensors fusion, like in [7],[8] or [9]. The IF uses an information matrix and an information vector to represent the co-variance matrix and the state vector usually used in a KF. This difference in representation makes the IF superior to the KF concerning multiple sensor fusion, as computations are simpler and no prior information of the system state is required [10] [11]. In addition, consistency test using log likelihood ratio (LLR), based on information matrix, permits to have a good balance between the detection probability and the false alarm probability.
Keywords:data fusion, Fault detection, information theory, localisation, GNSS, RAIM.
I.
INTRODUCTION
Autonomous navigation system requires a safety positioning system. When leading safety, positioning services not only need to provide an estimate of the vehicle location, but also uncertainty estimation. In practice, an upper bound on the positioning error, representing an integrity risk, is required to determine if a positioning system can be used for a given task. At GNSS (Global Navigation Satellite System) receiver level, standard approaches introduce a Fault Detection & Exclusion (FDE) stage to monitor the integrity of the estimation of the position. This is known as Receiver Autonomous Integrity Monitoring (RAIM) [1] [2]. Numerous FD studies have been conducted with real time sensors measurements. When we do not dispose of a sensor model, which is always the case with localisation sensors, FDE methods are approached in a statistical manner. Classically, stochastic approaches are developed through KALMAN filters [3], Particle Filter [4] or interval analysis [5]. These procedures for FD introduce residual testing
With the help of information filtering, the main interest of this paper is to ensure the reliability of the GNSS positioning in a navigation system. In this paper, a new GNSS integrity monitoring method is presented by applying a distributed IF and a LLR test. Specifically, the presented (FDE) scheme takes advantage of the highly LLR change between the information of each satellite and the mutual information of a set of visible satellites by a GNSS (GPS) receiver. The performance of the distributed IF is notable after reconfiguration of the data fusion procedure. This paper is organized as follows: Section 2 briefly introduces the general principle of distributed IF. In section 3, a detailed GPS integrity monitoring algorithm that employs distributed IF and ratio test is illustrated. A result using real GPS measurement
743
data is presented in section 4, followed by the conclusion in section 5. II.
𝐼(𝑘) is the associated information matrix. The couple (𝑖 𝑘 , 𝐼(𝑘)) represents the Information Contribution (IC) of the observation.
INFORMATION THEORY AND FILTERING
In multiple sensor problems:
A. Information filter
zi k = Hi k . x k + vi k
An Extended KF can easily be expressed in another form, called Nonlinear Information Filter (NIF) or inverse covariance filter [10]. Instead of working with the estimation of states 𝑥(𝑖|𝑗) and the variance 𝑃(𝑖|𝑗) , the IF deals with the information state vector 𝑦(𝑖|𝑗) and information matrix 𝑌(𝑖|𝑗), where: y 𝑖 𝑗 = P −1 𝑖 𝑗 . x 𝑖 𝑗
(1)
𝑌(𝑖|𝑗) = 𝑃 −1 (𝑖|𝑗)
(2)
(9)
i = 1…N
the estimate does not represent a simple linear combination of contributions from individual sensors : (10) 𝑥 𝑘 𝑘 ≠𝑥 𝑘 𝑘−1 + 𝑁 𝑖=1 𝑊𝑖 𝑘 . 𝑧𝑖 𝑘 − 𝐻𝑖 𝑘 . 𝑥 𝑘 𝑘 − 1 (with 𝑊𝑖 (𝑘) independent gain matrices). The innovation generated from each sensor is correlated. Indeed, they share common information through the prediction 𝑥 (𝑘|𝑘 − 1).
The information matrix is closely associated with the Fisher information. The physical mean of the Fisher information is the surface of a bounding region containing probability mass. It measures the compactness of a density function.
In information form, estimates can be constructed from linear combinations of observation information.
(3)
It is because information terms 𝑖𝑖 (𝑘) from each sensor are assumed to be uncorrelated. Now it is straightforward to evaluate the contribution of each sensor (unlike for a KF). Each sensor node simply generates the information terms 𝑖𝑖 (𝑘). These are summed at the update step of the NIF to produce a global information estimate.
𝑥 𝑘 = 𝐹 𝑘 .𝑥 𝑘 − 1 + 𝑤 𝑘
𝑦 𝑘 𝑘 = 𝑦 𝑘 𝑘−1 +
Where 𝑥(𝑘) is the state vector, 𝐹(𝑘) the state transition matrix and 𝑤(𝑘) the process noise.
𝑁 𝑖=1 𝑖𝑖
𝑘
(11)
The observation is also modeled: 𝑧 𝑘 = 𝐻 𝑘 . 𝑥 𝑘 + 𝑣(𝑘)
B. Mutual Information
(4)
Where 𝑧(𝑘) is the observation vector, 𝐻 𝑘 the observation matrix and 𝑣(𝑘) is a white noise.
To evaluate the IC of each sensor, a well known concept of Information theory can be used: the Mutual Information (MI). The MI 𝐼(𝑥, 𝑧) is an a priori measure of the information which will be gained by x with a set of observations z.
The update stage of the NIF is written :
𝐼(𝑥, 𝑧) is defined in [12] as:
𝑦 𝑘|𝑘 = 𝑦 𝑘|𝑘 − 1 + 𝑖 𝑘 𝑌 𝑘|𝑘 = 𝑌 𝑘|𝑘 − 1 + 𝐼 𝑘
(5) 𝐼 𝑥, 𝑧 = −𝐸 ln
(6)
𝑃 𝑥, 𝑧 𝑃 𝑥 .𝑃 𝑧 = −𝐸 ln
Where 𝑖 𝑘 = 𝐻 𝑇 𝑘 . 𝑅 −1 𝑘 . 𝑧(𝑘)
= −𝐸 ln
𝑃 𝑥𝑧 𝑃 𝑥
𝑃 𝑧𝑥 𝑃 𝑧
(12)
(7) In term of entropies, the MI is written:
𝑖(𝑘) is a vector containing the contribution in term of information from an observation 𝑧(𝑘). 𝐼 𝑘 = 𝐻𝑇 𝑘 . 𝑅−1 𝑘 . 𝐻𝑇 𝑘
𝐼 𝑥, 𝑧 = 𝐻 𝑥 + 𝐻 𝑧 − 𝐻 𝑥, 𝑧 = 𝐻 𝑥 − 𝐻 𝑥 𝑧
(8)
(13)
With 𝐻(𝑥), 𝐻(𝑧) the respective entropies of 𝑥 and 𝑧. 𝐻 𝑥 𝑧 and 𝐻 𝑧 𝑥 the conditional entropies.
744
III.
FAULTY
As well detailed in [13], the process model depends on the dynamical characteristics of the vehicle. The system is assumed to evolve according to the equation:
DETECTION AND EXCLUSION : APPLICATION TO GPS INTEGRITY MONITORING
GNSS integrity monitoring or Receiver Autonomous Integrity Monitoring (RAIM) is a technology developed to control the integrity of global navigation satellite system signals in a receiver system. During a long time, integrity was exclusively used in aviation or marine navigation. The integrity represents the ability to associate to a position a reliable indication of trust.
𝑥 𝑘 + 1 = 𝐴. 𝑥 𝑘 + 𝑤 𝑘 + 1
(14)
Where the state vector is composed of the eight following variables: 𝑥 𝑘 = 𝑋 𝑘 , 𝑋 𝑘 , 𝑌 𝑘 , 𝑌 𝑘 , 𝑍 𝑘 , 𝑍 𝑘 , 𝑐𝜕𝑡 𝑘 , 𝑐𝜕𝑡 𝑘
With the development of different safety-critical urban applications, the notion of integrity became an important field of research, in particular in urban canyon, where satellites signals are subject to hostile environment.
(15)
[𝑋, 𝑌, 𝑍] representing the receiver position,[𝑋, 𝑌, 𝑍]the velocity in Earth Centered Earth Fixed (ECEF) frame, 𝜕𝑡 the clock range and 𝜕𝑡 the clock drift.
Usually, RAIM algorithms use a comparison of the result of a main equation system and subsystems results [4], we propose to use the information theory to quantify the information contribution of each satellite (Figure 1). In other words, we propose to detect and to exclude faulty satellite upstream, meaning before the equation system resolution.
The state transition matrix is given by: 1 0 0 0 0 0 0 0
𝐴=
𝑇 1 0 0 0 0 0 0
0 0 1 0 0 0 0 0
0 0 𝑇 1 0 0 0 0
0 0 0 0 1 0 0 0
0 0 0 0 𝑇 1 0 0
0 0 0 0 0 0 1 0
0 0 0 0 0 0 𝑇 1
(16)
Where 𝑇 is the sample period (in our case 1 second). The measurement noise 𝑤 . is assumed to be time invariant: 𝑤 . = 𝑇 2 𝜍𝑥 2
𝑇 2 𝜍𝑦 2
2
, 𝑇𝜍𝑥 2 ,
2
, 𝑐𝜍𝑔 2 𝑇
𝑐𝜍𝑔 2 𝑇 2
2
, 𝑇𝜍𝑦 2 ,
𝑇 2 𝜍𝑧 2 2
, 𝑇𝜍𝑧 2 , 𝑐𝜍𝑓 2 𝑇 +
(17)
Where 𝜍𝑥 2 , 𝜍𝑦 2 , 𝜍𝑧 2 represent the process noise variances related to the states 𝑥, 𝑦 and 𝑧, 𝜍𝑓 and 𝜍𝑔 are the variances related to clock offset and velocity, and 𝑐 the speed of light . Making the assumption that 𝑤~𝑁 0, 𝑄 means that 𝑤 is assumed to be a white Gaussian noise with covariance 𝑄 :
Figure 1. Satelllite Information contribution for GNSS Monitoring
A. State space representaion for GNSS Positionning GNSS positioning with pseudo-range is a Time of Arrival method [5]. Pseudo-ranges are the distances between visible satellites and the receiver plus the unknown difference between the receiver clock and the GNSS time. Thus, GNSS positioning is a four dimensional problem: the 3D coordinates (𝑥𝑖 , 𝑦𝑖 , 𝑧𝑖 ) of the user and the clock offset 𝑑𝑡𝑖 are unknown.
𝑄=
745
0 0 0 0 0 𝛼𝜍𝑥 2 𝛽𝜍𝑥 2 0 0 0 2 2 0 0 0 0 𝛽𝜍𝑥 𝛾𝜍𝑥 2 2 0 0 𝛼𝜍 0 0 𝛽𝜍 𝑥 𝑥 0 0 2 0 0 2 0 0 0 0 𝛽𝜍𝑥 𝛾𝜍𝑥 2 2 0 0 𝛼𝜍 𝛽𝜍 𝑥 𝑥 0 0 0 0 0 0 0 0 𝛽𝜍𝑥 2 𝛾𝜍𝑥 2 0 0 2 + 𝛼𝜍 2 𝛽𝜍 2 𝛾𝜍 𝑓 𝑔 𝑔 0 0 0 0 0 0 2 2 𝛽𝜍 𝛾𝜍 0 0 0 0 𝑔 𝑔 0 0
(18)
…
𝜍1
With 𝑐2𝑇3 3
(19)
𝑐2 𝑇2 𝛽= 2
(20)
𝛼=
𝛾=
⋱ 𝑅(. ) =
0
(21)
𝑇
𝑥𝑖 − 𝑥 𝑆 2 + 𝑦𝑖 − 𝑦 𝑆 2 + 𝑧𝑖 − 𝑧 𝑆 𝑐. 𝑑𝑡𝑖 − 𝑑𝑡 𝑠 + 𝛿𝜌𝑖𝑆𝐼𝑜𝑛𝑜 + 𝛿𝜌𝑖𝑆𝑇𝑟𝑜𝑝𝑜
𝐻=
With 𝑗 ∇𝑥
2
𝐻 𝑋 =
+
𝑗
=
1 0 ∇𝑦 0 ∇1𝑧 0 ⋮ ⋮ ⋮ ⋮ ⋮ 𝑗 0 ∇𝑦 0 ∇𝑧𝑗 0 ⋮ ⋮ ⋮ ⋮ ⋮ 0 ∇𝑦𝑛 0 ∇𝑧𝑛 0
1 ⋮ 1 ⋮ 1
(24)
2
+ 𝑧 𝑆 − 𝑧𝑝𝑟
(25)
(26)
2
1 𝑃(𝑋) ln 2 𝑃(𝑋|𝑍)
(30)
(31)
A Global Information Mutual Information (GOMI) can be defined. 𝑃(𝑋) is the prediction covariance and 𝑃(𝑋|𝑍) is the updated covariance. The GOMI becomes, in information form: 1 𝑌(𝑘|𝑘) ln 2 𝑌(𝑘|𝑘 − 1)
1 𝑌 𝑘 𝑘−1 + 𝑁 𝑖=1 𝐼𝑖 (𝑘) 𝐼 𝑋, 𝑍 = ln 2 𝑌(𝑘|𝑘 − 1)
And + 𝑦 𝑆 − 𝑦𝑝𝑟
(29)
𝐼 𝑋, 𝑍 =
𝐼 𝑋, 𝑍 =
𝜕𝑅𝑗𝑠 𝑗 𝑠 ∇𝑧 = = −(𝑧 𝑆 − 𝑧𝑝𝑟 ) 𝑅𝑝𝑟 𝜕𝑧
2
𝑑 ln(2𝜋) 𝜀 + 𝑑. + ln 2 2 2
𝑑 ln 2𝜋 𝑃(𝑋) 𝑑 ln 2𝜋 + 𝑑. + ln − ( + 𝑑. 2 2 2 2 2 𝑃(𝑋|𝑍) + ln ) 2 It becomes a basic Log Likelihood Ratio :
(23)
𝜕𝑅𝑗𝑠 𝑠 = −(𝑦 𝑆 − 𝑦𝑝𝑟 ) 𝑅𝑝𝑟 𝜕𝑦
𝑥 𝑆 − 𝑥𝑝𝑟
𝜍𝑛
From Eq.13 & Eq. 29, the MI in our case is defined:
0 ⋮ 0 ⋮ 0
𝐼 𝑋, 𝑍 =
𝑠 𝑅𝑝𝑟 =
(28)
𝑁 𝑋 . ln 𝑁 𝑋 . 𝑑𝑋
(22)
𝜕𝑅𝑗𝑠 𝑠 = = −(𝑥 𝑆 − 𝑥𝑝𝑟 ) 𝑅𝑝𝑟 𝜕𝑥
∇𝑦 =
…
⋮ ⋱
As proved in [14] the entropy, for a multivariate Gaussian distribution (which is assumed to be the case in Eq. 14 for our study), goes as the log-determinant of the covariance. Precisely, the differential entropy of a d-dimensional random vector 𝑋 drawn from the Gaussian 𝑁(𝜇, 𝜀) is:
Being non-linear, the observation model is linearised around a predicted state [𝑋𝑝𝑟 ] to obtain an observation matrix: ∇1𝑥 ⋮ 𝑗 ∇𝑥 ⋮ ∇𝑥𝑛
𝜍𝑗
B. Mutual Information for FDE
The observation [𝑥𝑖 , 𝑦𝑖 , 𝑧𝑖 ] of each satellite 𝑖 , is calculated thanks to broadcasted ephemeris. The atmospheric errors are also modeled 𝛿𝜌𝑖𝑆𝐼𝑜𝑛𝑜 , 𝛿𝜌𝑖𝑆𝑇𝑟𝑜𝑝𝑜 . The pseudo-range can be modeled as follow: 𝑅𝑖𝑠 =
⋮
0
(32)
(33)
We also define the Partial Observation Mutual Information (POMI) which will take in account only one information term of sensor 𝑖.
(27)
Each satellite noise is assumed to be uncorrelated with all others. So, the noise described as follow 𝑣~𝑁 0, 𝑅 is simply represented by its diagonal time-invariant matrix R:
1 𝑌 𝑘 𝑘 − 1 + 𝐼𝑖 (𝑘) 𝐼𝑖 𝑋, 𝑍 = ln 2 𝑌(𝑘|𝑘 − 1)
(34)
The GOMI will prove to be an adapted residual for fault detection, whereas the POMI will isolate the faulty observation.
746
C. Principle of the FDE method
thresholding stage in order to detect the inconsistency of one or several satellites. In case of fault detection, a POMI (Eq. 34) is computed for each satellite. These different POMIs are used to isolate the faulty satellites measurements in order to be excluded from the correction step of the NIF algorithm.
The structure of the proposed GNSS integrity monitoring method is illustrated in figure 2 which correspond to a classical prediction/correction filtering using IF but including a FDE stage.
IV.
EXPERIMENTAL RESULTS
In order to test the performance of the RAIM developed approach, real data acquisition has been carried out with CyCab vehicle produced by Robosoft (www.robosoft.fr/) with several embedded sensors. In this work, measurements of GPS RTK Thales Sagitta 02 system and open GPS Septentrio Polarx2e@ (Figure 4) are used.
Figure 2. Classical GNSS Integrity Monitoring schema
In the proposed FDE approach, described in the following flowchart, in figure 3, only five measurements are needed for the integrity monitoring.
Figure 4.
Experimental vehicle.
The data acquisition has been carried out around LORIA Lab. During these experiments we remarked, as shown in figure 5, that when the positionning system uses all visible satellite without a FDE stage, one satellite introduces a bias in the positionning process. It seems that this satellite was in a bad geometric configuration in respect to the building in front (Bat C in the figure 4, 5 and 6). To compare, the test trajectory reference with centimetric accuracy (GPS RTK) is plotted in green in figure 6. This trajectory is about eighty meters length.
Figure 3. Information Theory based proposed method
With n measurements, n Information Contribution (IC1 … ICn) (Eq. 7 & Eq.8) are computed. A GOMI (Eq. 33) is then computed using the n IC. The GOMI permits to have an auto-
747
Figure 5.
Trajectory after fusion without FDE
Figure 7. Global Observation Mutual Information for Detection
The fault exclusion step is for each satellite. In fact, inconsistency of the faulty One can see the surges of blue. Figure 6.
realised using the POMI computed the POMI permits to identify the satellite like discribed in figure 8. POMI of satellite 6 represented in
Reference trajectory given by RTK GPS
In figure 7 and 8, the results of the detection and exclusion process are presented for the set of visible satellites during the test trajectory. GOMI concept, introduced in section 2 helps to auto-threshold information contribution of all satellites. After few steps of initialization, the contribution of each satellite goes to ―stabilize‖ around a value. Indeed, a satellite, if still visible, should not bring a contribution different to the information gives by the steps before. Figure 7, one can easily see the different surges of information contribution. That because a pseudo-range is generally overestimated when the corresponding satellite waves is subject to multi-path. Thus, the IC of the faulty satellite is also overvalued. This IC becomes inconsistent, and need to be excluded.
Figure 8.
Partials Observation Mutual Information for Exclusion
In figure 9, is shown the performance of the fusion with FDE after exclusion of satellite 6 from the fusion procedure. We note that the trajectory in figure 9 is close to the GPS RTK trajectory showed in figure 6.
In this test, we detected only one satellite real errors, but the proposed approach can be generalized for multiple faulty satellites.
748
introduced by each satellite observation compared to the expected information.
Figure 9.
The evaluation of FDE is conducted through simulation using real GPS measurements data. In the presented test trajectory, the measured data from GPS contain the well known wave’s multipath error illustrated with an abnormal bias. Based on real data, results demonstrate performance of the proposed approach in detecting GPS measurements faults and fusion method reconfiguration. In future work, our objective is to integrate dead reckoning sensors in order to develop a tightly coupled data fusion method. These sensors are known to introduce gradually increasing errors (drift). The LLR is well adapted to detect this kind of errors.
Trajectory after fusion with FDE
Finally, the figure 10 shows performance of the proposed approach with faulty satellite exclusion in blue. This figure shows the position computation in the ECEF frame for each axis (X,Y,Z). In red, are the position computation on ECEF frame without faulty satellite exclusion.
REFERENCES Erreur ! Source du renvoi introuvable. [1]
R. Grover Brown, Gerald Y. Chin, ―GPS RAIM: Calculation of Threshold and Protection Radius Using Chi-Square Methods-A Geometric Approach‖, in. Global Positioning System: Inst. Navigat., vol. V, pp. 155– 179, 1997 [2] T. Walter, P. Enge. ―Weighted RAIM for Precision Approach.‖ Proceedings of the ION-GPS 1995, Palm Springs, CA, 1995. [3] B. W. Parkinson, and P. Axelrad, ―Autonomous GPS integrity monitoring using the pseudorange residual‖, Navig., J. Inst. Navig., vol. 35, no. 2, pp. 255–274, 1988. [4] J. Ahn & al., ―GPS Integrity Monitoring Method Using Auxiliary Nonlinear Filters with Log Likelihood Ratio Test Approach‖, Journal of Electrical Engineering & Technology Vol. 6, No. 4, pp. 563~572, 2011. [5] V. Drevelle and Ph. Bonnifait ,―Global Positioning in Urban Areas with 3D Maps‖ Proceedings of the 2011 IEEE Intelligent Vehicles Symposium, Baden-Baden, Allemagne, pp. 764-769, 2011 [6] Y. C. Lee, ―Analysis or range and position comparison methods as a means to provide GPS integrity in the user receiver,‖ in Proceedings of the Annual Meeting of the Institute of Navigation, Seattle, WA, Jun. 1986, pp. 1–4, 1986. [7] B. Grocholsky, H. Durrant-Whyte, and P. Gibbens, ―An informationtheoretic approach to decentralized control of multiple autonomous flight vehicles,‖ Proc. SPIE: Sensor Fusion and Decentralized Control in Robotic Systems III, vol. 4196, pp. 348–359, Oct. 2000. [8] L. Wang, Q. Zhang , H. Zhu , and L. Shen, ―Adaptive consensus fusion estimation for msn with communication delays and switching network topologies‖. Decision and Control (CDC), 2010 49th IEEE Conference on, pages 2087 –2092,2010. [9] G. Liu, F. Wörgötter, and I. Markeli´c, ―Nonlinear estimation using central difference information filter‖. In IEEE Workshop on Statistical Signal Processing, pages 593–596, 2011. [10] S. Grime and H. F. Durrant-Whyte. ―Data Fusion in Decentralized Sensor Networks‖. Control Engineering Practice, 2:849-863, 1994. [11] D.-J. Lee, ―Nonlinear estimation and multiple sensor fusion using unscented information filtering‖. Signal Processing Letters, IEEE, 15:861 – 864, 2008. [12] T.M. Cover and J.A. Thomas, Elements of information theory. John Wiley & Sons, New York, NY, Inc., Chapter 9 Theroem 9.4.3, 1991. [13] S. Cooper and H. F. Durrant-Whyte. ―A Kalman filter model for GPS navigation of land vehicles‖. In: IEEE/RSJ/GI International Conference on Intelligent Robots and Systems, p. 157—163, 1994. [14] T.M. Cover and J.A. Thomas, Elements of information theory. John Wiley & Sons, New York, NY, Inc., Chapter 2 Definition 2.3, 1991.
Figure 10. Exclusion of a faulty satellite in GNSS positioning
V.
CONCLUSION AND FUTURES WORKS
This paper proposes a reliable multi sensor data fusion approach with integrity monitoring. This approach integrates Fault Detection stage by using distributed NIF and LLR test using mutual information. The proposed method makes it possible to detect fault without heuristic thresholing step. It permits an automatic reconfiguration of the fusion procedure in order to exclude the faulty measure. The proposed approach is applied for a GPS integrity monitoring. Measurements were pseudoranges from the GPS satellites. The basic concept is to use the Information Filter in a distributed architecture. The likelihood function is established and examined by integrating state estimate from distributed Information Filters. Thus, the LLR test is used in fault detection, which compares the information
749
