EKF based Data Fusion using Interval Analysis via Covariance

Intersection, ML and a Class of OGK Covariance Estimators

Samuel B. Lazarus

, Antonios Tsourdos

, Jo˜ao Sequeira

and Al Savvaris

Cranﬁeld University, Cranﬁeld, U.K.

Instituto Superior T´ecnico, Lisboa, Portugal

Keywords:

Multiple Sensor Fusion, Data Fusion, EKF based Navigation, Interval Analysis (IA), Robust Navigation,

Covariance Intersection (CI), Maximum Likelihood (ML), Orthogonal Gnanadesikan-Kettenring (OGK).

Abstract:

This paper addresses the comparison of robust estimation of a covariance matrix in vehicle navigation task to

express the uncertainty when fusing information from multiple sensors. The EKF estimates are fused with the

Interval Analysis estimates and further the results are fused using the Covariance Intersection (CI), Maximum

Likelihood (ML) and a class of Orthogonal Gnanadesikan-Kettenring (OGK) estimators. The simulation re-

sults presented show that the variation between CI and OGK and the correlation between sensors are signiﬁcant

in the presence of outliers.

1 INTRODUCTION

The data fusion from any two different sources is tra-

ditionally used to increase the accuracy of the mea-

surement being performed, and to overcome unrelia-

bility in sensors or uncertainty in sensor outputs, so

that the resulting information is more accurate, more

complete and reliable to the result of an emerging

view (Carvalho and Rezende, 2004). The data fusion

technique is claimed to be applicable to the fusion of

sensor measurements, data estimates, or similar quan-

tities that can be described in terms of a Gaussian

probability density function. The Covariance Inter-

section technique is related to a more general data fu-

sion technique that can fuse with any pair of probabil-

ity density functions. The principal application of Co-

variance Intersection is an adjunct to Kalman ﬁlters

where there is the potential for the data to be highly

correlated. Hence fusion can reduce the system’s de-

pendence on invalid prior assumptions and make the

system more robust.

Many researchers have proposed their algorithms

by combining different sources of sensors. Smooth-

ing approaches are also used as a viable alternative

to extended Kalman ﬁlter (EKF)- based solutions. In

particular, approaches have been looked at as that fac-

torize either the associated information matrix or the

measurement Jacobian into square root form. Such

techniques have several signiﬁcant advantages over

the EKF: they are faster yet exact; and yield the en-

tire robot trajectory at lower cost for a large class

of SLAM problems. Mixing the information from

diverse sources, which are obtained with a differ-

ent conﬁdence degree, is at the core of a multiple

of problems in robotics. Multiple techniques for in-

formation fusion have been in use for many years

in robotics, ranging from the deterministic EKF to

the stochastic approaches using particle ﬁlters (for

instance (Montemerlo et al., 2002), (Mourikis and

Roumeliotis, 2006), (Vadakkepat and Jing, 2006),

(Walter and Leonard, 2004)). At the output of the

fusion process, a covariance matrix that expresses the

variances of the estimation error along with the differ-

ent degrees of freedom is produced.

In this paper the ﬁrst mile stone is achieved by

fusing the covariance matrix of the EKF estimates

(Time Update step) with the Interval position of the

vehicle to improve the sensor uncertainties in esti-

mating the vehicle’s accurate position. Then the Co-

variance Intersection (CI) algorithm given in (Julier

and Uhlmann, 1997), (Ashokaraj et al., 2009) is

used here as a data fusion algorithm as described

in (Lazarus et al., 2008). It is further extended by

employing a different data fusion estimators such as

Maximum Likelihood (ML) and a class of Orthogo-

nal Gnanadesikan-Kettenring (OGK) estimators from

(Gnanadesikan and Kettenring, 1972). The obtained

results are compared with one another and the change

in uncertainty levels between the resultant covariance

matrix of each estimators are well studied.

610

B. Lazarus S., Tsourdos A., Sequeira J. and Savvaris A..

EKF based Data Fusion using Interval Analysis via Covariance Intersection, ML and a Class of OGK Covariance Estimators.

DOI: 10.5220/0004040406100613

In Proceedings of the 9th International Conference on Informatics in Control, Automation and Robotics (ICINCO-2012), pages 610-613

ISBN: 978-989-8565-21-1

 2012 SCITEPRESS (Science and Technology Publications, Lda.)

2 CI, & OGK ESTIMATORS

The covariance intersection algorithm (CI) has a con-

vex combination of the means and the covariance

in the information space. CI estimates are shown

that it is an optimal method when the cross corre-

lation between the measurements being fused is un-

known. This advantage of this additional information

exchange is that estimates on the cross correlation be-

tween them can then be integrated in the fusion pro-

cess. The class of OGK estimators used in this paper

is described in Table (1). It is a variation from the

method described in (Copt and Victoria-Feser, 2003)

and combines both information on the covariance es-

timates and on the data that yielded such covariances.

For the purpose of experiments this is set to ω = 1.

The underlying idea of the process is to extract vari-

ances accounting for the distances between the mea-

surements. If one of the axis is much bigger than the

other and the variances along this axis are small then it

is likely that the measurements that induce the length

of the bigger axis are but outliers.

To summarize the process, the OGK estima-

tor starts by scaling the input data (step 2) and

computing an initial covariance estimate using the

Gnanadesikan-Kettering estimator (step 3). This ini-

tial estimate is used to obtain a new basis (step 4),

formed with the eigenvectors, where the scaled data

are projected and the new variances are computed

along each of the axis of the new frame (step 5). The

data are then reverted back to the original frame (step

6).

3 IA BASED POSITION

ESTIMATION

This section describes the vehicle’s position estima-

tion using interval analysis (Kieffer et al., 2000) with

the sensor readings from the laser sensors. The main

advantage of this method is that, it guarantees on the

bounds and that makes the system less sensitive to

the problem of consistency of typical ﬁlters such as

the Extended Kalman Filters. Also it handles the

problem without any linearization. The interval po-

sition estimation using SIVIA (Set Inversion Via In-

terval Analysis) (Jaulin and Walter, 1993) reported

in (Ashokaraj, 2004) to estimate the interval posi-

tion of the vehicle. The detailed description of this

algorithm is described in (L´evˆeque et al., 1997) and

(Jaulin et al., 2001). SIVIA assumes that it has a large

initial search box [x]

which is guaranteed to include

A. The SIVIA basically has four steps and the two

subpavings A and A which are initialized with empty

box so as to identify the exact interval position within

that box. The four basic steps of SIVIA based on the

inclusion functions are given below:

• [x] does not belong to A, if [f]([x]) has an empty

intersection with B i.e [f]([x]) ∩ B =

• [x] is included in the solution subpaving A, if

[f]([x]) is completely inside B and therefore they

are stored in A and A.

• [x] is judged to be undetermined, thus implying

that [x] may only include part of the solution set if

[f]([x]) has a non-empty intersection with B, but

is not completely inside B. If [x] has a width w

greater than ε the pre-speciﬁed precision param-

eter, then the box is bisected generating two off-

springs and the tests are again applied recursively

for these offsprings.

• Finally if the box has a width smaller than the pre-

speciﬁed precision parameter ε and at the same

time they are found to be undetermined, then it is

considered to be small enough to be stored in the

outer approximation A of A.

4 EKF BASED DATA FUSION

As described earlier, in this paper the three different

data fusion estimators are presented namely, CI, ML,

OGK. The vehicle is assumed that it operates in a par-

tially known environment where the surrounding ob-

stacles are known to some extent. It is also assumed

that the vehicle velocity is assumed to be constant

and the vehicle is equipped with INS sensors, laser

and GPS receiver. An INS with a nonlinear extended

Kalman ﬁlter is used to estimate the heading angle

and the position of the vehicle. The INS provides the

information about the vehicle’s position, velocity and

heading angle (yaw angle). To estimate the errors in

the INS states a Kalman ﬁlter is used which utilizes

the measurements from other estimated sources (X &

Y position from IA). The Kalman ﬁlter uses an INS

error model which gives the optimal Kalman gain.

This Kalman gain is used with the innovations(X & Y

positions) to estimate the errors in the INS estimates

(Lazarus et al., 2008).

As the vehicle moves, based on the measurements

received from the INS senors the time-update stepin

the Kalamn ﬁlter is updated. When the vehicle passes

through any of the known surrounding obstacles then

the fusion algorithm is implemented. In order to ac-

complish this task, initially the vehicle’s interval posi-

tion is estimated using IA based SIVIA method which

is relative to the known obstacle. Basically this fu-

sion algorithm using EKF represents the infertilities

EKFbasedDataFusionusingIntervalAnalysisviaCovarianceIntersection,MLandaClassofOGKCovariance

Estimators

611

Table 1: The OGK covariance estimator (ω is the class parameter).

0 Let σ(·) be a standard deviation function applied to its argument

1 Let X = (X

, . . . , X

) ∈ R

n×p

be the set of n observations, each of dimension p

2 Let D = diag{(}σ(X

)), j = 1, . . . , p and deﬁne Y = XD

−T

3 Compute U =





(

(σ(X

+ X

)

− σ(X

− X

)

) j 6= k

σ(X

)

j = k

4 Compute E such that U = EΛE

−1

with Λ the diagonal matrix with the eigenvalues of U

5 Let Z = YE and A = ωED

−1

, with ω a scalar, and Γ = diag(σ(Z

)

), j = 1, . . . , p

6 The covariance estimate is Σ = A

ΓA

from two different sensors. The ﬁst source of infor-

mation is obtained from the INS sensors and its uncer-

tainties are represented in the form of P

matrix (i.e.,

the covariance matrix in EKF in time step). The sec-

ond source of information is obtained from the Inter-

val analysis method where the uncertainties are rep-

resented in the form of interval (i.e., Interval box of

the vehicle’s position using laser range sensor). For

the sake of simplicity in this work a set of random

data are generated within this interval by using the

mid point of this interval box thereby a covariance

is constructed which represents the uncertainty in the

interval position of the vehicle.

Finally the information obtained from these two

different sources are fused with the CI algorithm so

as to perform the measurement-update process in the

Kalman Filter. Further, the same process of generat-

ing a random set of data is applied for the EKF esti-

mated covariances as well. then the constructed two

different covariance matrices which use the random

data within two different uncertainties are fused using

the ML algorithm. Then, this set of two different ran-

dom data is given as an input to the OGK estimator in

order to fuse this two different source of information.

The resultant covariance matrix is used to update the

Kalman Filter’s measurement-update step.

5 SIMULATION RESULTS

The simulation results of the above explained algo-

rithm is given in this section. The task of vehicle

navigation is achieved by updating the EKF at each

iteration (i.e., the time and measurement updates).

Initially a sequence of ten random data is generated

within the interval region thereby a covariance is es-

timated from the IA estimated position so as to fuse

this covarince matrix with the Kalman predicted co-

variance matrix (i.e. time-update step). Secondly

these two covariances are fused using CI algorithm.

Thirdly a same set of random data is also generated

within the region of the Kalman estimated covariance

and thereby a covariance matrix is constructed using

this random data. Finally, these two set of covariances

are fused using the ML and the OGK estimators. The

simulation results of some of the iterations are given

in ﬁgures 1 - 3. As can be observed from these ﬁg-

−1 −0.8 −0.6 −0.4 −0.2 0

2.4

2.5

2.6

2.7

2.8

2.9

3.1

3.2

EKF

data−1

data−2

ML 1

ML 2

CI 1,2

ML 1,2

(a) Iteration 2

−0.9 −0.8 −0.7 −0.6 −0.5 −0.4 −0.3 −0.2 −0.1

4.4

4.5

4.6

4.7

4.8

4.9

5.1

EKF

data−1

data−2

ML 1

ML 2

CI 1,2

ML 1,2

(b) Iteration 6

Figure 1: The resultant covariances using CI, ML, and OGK

at each iterations-1.

2.2 2.4 2.6 2.8 3 3.2 3.4

7.5

7.6

7.7

7.8

7.9

8.1

8.2

8.3

8.4

8.5

EKF

data−1

data−2

ML 1

ML 2

CI 1,2

ML 1,2

(a) Iteration 12

4 4.5 5 5.5 6 6.5 7 7.5 8

6.5

7.5

8.5

9.5

EKF

data−1

data−2

ML 1

ML 2

CI 1,2

ML 1,2

OGK 1,2

(b) Iteration 19

Figure 2: The resultant covariances using CI, ML, and OGK

at each iterations-2.

6 7 8 9 10 11 12

1.5

2.5

3.5

4.5

5.5

EKF

data−1

data−2

ML 1

ML 2

CI 1,2

ML 1,2

OGK 1,2

(a) Iteration 27

−2 0 2 4 6 8

−4

−3

−2

−1

EKF

data−1

data−2

ML 1

ML 2

CI 1,2

ML 1,2

OGK 1,2

(b) Iteration 39

Figure 3: The resultant covariances using CI, ML, and OGK

at each iterations-3.

ICINCO2012-9thInternationalConferenceonInformaticsinControl,AutomationandRobotics

612

ures, since the Kalamn estimated covariance is negli-

gible (small), the OGK algorithm is not implemented

in the initial iterations. As the vehicle moves, the un-

certainties in the system noise will rapidly increas;

this requires the necessitates the implementation of

the data fusion algorithm. It is also observed that

the Kalman estimated covariance is emerging bigger

in each of the iterations by increasing the region of

uncertainties. Whenever the data fusion algorithm is

applied the region of this uncertainties is rapidly re-

duced by performing the measurement-update in the

Kalman ﬁlter. The last stages of iterations show that

the OGK algorithm is robust when the Kalman esti-

mates are unpredictable within the assumed distribu-

tions. In nutshell, the presented algorithms are com-

plementary in the sense that they compensate for each

other’s limitations, so that the resulting performance

is much better than of its individual techniques, which

in turn, provide more accurate position information of

the vehicle.

6 CONCLUSIONS

The paper presents robust data fusion techniques via

CI and a particular class of OGK covariance esti-

mators for fusion of information from two different

sources namely EKF and IA. The CI approach uses

the covariance matrices of the data sources whereas

the OGK uses an estimate of the joint covariance and

information from the measurements themselves. The

comparison between the two estimators is based on

the 2-norms. This measure combines information re-

lated to the volume of the error ellipsoids and their

eccentricity. The analysis on the relevant bounds for

the two measures shows that, in worst case conditions,

there are regions of the spectrum of the covariance

matrices in which each of the estimators outperforms

the other. For generic applications, a hybrid of the

two estimators may then provide the best results.

The application of this OGK estimation technique

to 3D needs a recalculation of the bounds involved.

Still, the same basic principles apply. The ongoing

work also includes the testing of classes of OGK es-

timators in the information fusion problem that are

applied to a number of ground and aerial vehicle in-

volved in a mapping mission.

REFERENCES

Ashokaraj, I. A. R. (2004). Feature based navigation and

localisation using interval analysis. Phd thesis, Cran-

ﬁeld University.

Ashokaraj, I. A. R., Silson, P. M. G., Tsourdos, A., and

White, B. A. (2009). Robust sensor-based navigation

for mobile robots. IEEE Transactions on Instrumen-

tation and Measurement, 58(3):551–556.

Carvalho, K. F. A. C. D. and Rezende, S. O. (2004). Com-

bining intelligent techniques for sensor fusion. Ap-

plied Intelligence, 20:199213.

Copt, S. and Victoria-Feser, M. (2003). Fast algorithms for

computing high breakdown covariance matrices with

missing data.

Gnanadesikan, R. and Kettenring, J. (1972). Robust esti-

mates, residuals, and outlier detection with multire-

sponse data. Biometrics, 28(1):81–124.

Jaulin, L., Kieffer, M., Didrit, O., and Walter, E. (2001).

Applied Interval Analysis with examples in parame-

ter and state estimation robust control and robotics.

Springer-Verlag, London.

Jaulin, L. and Walter, E. (1993). Set inversion via interval

analysis for nonlinear bounded-error estimation. Au-

tomatica, 29(4):1053 to 1064.

Julier, S. J. and Uhlmann, J. K. (1997). A non-divergent

estimation algorithm in the presence of unknown cor-

relations. In American Control Conference, pages

2369–2373, Albuquerque, New Mexico.

Kieffer, M., Jaulin, L., Walter, E., and Meizel, D. (2000).

Robust autonomous robot localization using interval

analysis. Reliable Computing, 3(6):337 to 361.

Lazarus, S. B., Tsourdos, A., Silson, P., White, B. A.,

and Zbikowski, R. (2008). Unmanned aerial vehi-

cle navigation andmapping. Proc.IMechE. Part G: J.

Aerospace Engineering, 222(246):531 – 548.

L´evˆeque, O., Jaulin, L., Meizel, D., and Walter, E. (1997).

Vehicle localization from inaccurate telemetric data: a

set-inversion approach. In 5th IFAC Symp on Robot

Control SY.RO.CO. ’97, Nantes, France, volume 1,

pages 179 – 186.

Montemerlo, M., Thrun, S., Koller, D., and Wegbreit, B.

(2002). FastSLAM: A Factored Solution to the Si-

multaneous Localization and Mapping Problem. In

Procs. of the 8th AAAI National Conf. on Artiﬁcial In-

telligence. Edmonton, Alberta, Canada.

Mourikis, A. I. and Roumeliotis, S. I. (2006). Predicting

the Performance of Cooperative Simultaneous Local-

ization and Mapping (C-SLAM). The International

Journal of Robotics Research, 25(12):1273–1286.

Vadakkepat, P. and Jing, L. (2006). Improved particle ﬁlter

in sensor fusion for tracking randomly moving object.

IEEE Transactions on Instrumentation and Measure-

ment, 55(5):1823–1832.

Walter, M. and Leonard, J. (2004). An Experimental Inves-

tigation of Cooperative SLAM. In Procs. of the 5th

IFAC Symposium on Intelligent Autonomous Vehicles,

(IAV 2004). Lisbon, Portugal.

EKFbasedDataFusionusingIntervalAnalysisviaCovarianceIntersection,MLandaClassofOGKCovariance

Estimators

613