dominique gruyer and alain lambert* mathias perrollaz denis … · 2014. 1. 30. · the covariance...

24 Int. J. Vehicle Autonomous Systems, Vol. 12, No. 1, 2014

Copyright © 2014 Inderscience Enterprises Ltd.

Experimental comparison of Bayesian positioning methods based on multi-sensor data fusion

Dominique Gruyer and Alain Lambert* IFSTTAR IM, LIVIC, F-78000 Versailles, France Email: [email protected] Email: [email protected] *Corresponding author

Mathias Perrollaz INRIA Grenoble Rhone-Alpes, 38334 Saint Ismier, France Email: [email protected]

Denis Gingras Université de Sherbrooke, 2500 Boulevard de l’Université, Sherbrooke, QC J1K2R1, Canada Email: [email protected]

Abstract: Localising a vehicle consists in estimating its position state by merging data from proprioceptive sensors (inertial measurement unit, gyrometer, odometer, etc.) and exteroceptive sensors (GPS sensor). A well-known solution in state estimation is provided by the Kalman filter. However, due to the presence of nonlinearities, the Kalman estimator is applicable only through some recursive variants among which the Extended Kalman filter (EKF), the Unscented Kalman Filter (UKF) and the Divided Differences of first and second order (DDI and DD2). We have compared these filters using the same experimental data. The results obtained aim to rank these approaches by their performances in terms of accuracy and consistency.

Keywords: localisation; mobile robotic; Kalman filter; EKF; UKF; DDI; DD2; sensor fusion; vehicle positioning.

Reference to this paper should be made as follows: Gruyer, D., Lambert, A., Perrollaz, M. and Gingras, D. (2014) ‘Experimental comparison of Bayesian positioning methods based on multi-sensor data fusion’, Int. J. Vehicle Autonomous Systems, Vol. 12, No. 1, pp.24–43.

Biographical notes: Dominique Gruyer received the MS and PhD degrees respectively in 1995 and 1999 from the University of Technology of Compiègne. Since 2001, he is a researcher at INRETS (now IFSTTAR), into the perception team (leader since 2010) of the LIVIC laboratory (COSYS

Experimental comparison of Bayesian positioning methods 25

department) and he works on the study and the development of multi-sensors/ sources data fusion. His works enter into the conception of on-board driving assistance systems (obstacles detection and tracking, extended perception, and robust ego-localisation) and SiVIC platform. He was/is involved in several European and French projects (HAVEit, Isi-PADAS, CVIS, CARSENSE, eMOTIVE, LOVe, ARCOS, MICADO, ABV, eFuture, CooPerCom).

Alain Lambert received the MSc degree in Control Sciences and the PhD degree in Robotics from the Université de Technologie de Compiégne, France, in 1994 and 1998, respectively. From 1999 to 2002, he has been an Assistant Professor at the ESIEE-Amiens (Ecole Supérieur d’Ingénieur en Electronique et Electrotechnique), France. Since 2002, he is Associate Professor at the Université Paris Sud, France, and a member of the ACCIS Research group at IEF (Institute of Fundamental Electronics). Since 2011, he is on leave as a senior researcher at LIVIC (Laboratory on interactions between vehicles, Infrastructure and drivers), a laboratory of the IFSTTAR (French institute of science and technology for transport). His research interests include perception, localisation and path planning.

Mathias Perrollaz graduated from the National Polytechnical Institute of Grenoble (INPG), France in 2003, with a major in signal and image processing. He worked on ITS at the LIS (CNRS) in Grenoble and with the perception team of LIVIC (INRETS). He received the PhD degree from the University of Paris-6 (UPMC) in 2008 for his work on multi-sensor obstacle detection. He has been developing probabilistic methods for ITS at INRIA Grenoble since 2009. He worked on perception for robotics at Ohio Northern University, OH, USA in 2011. Mathias taught in the Paris-10 and Grenoble-2 Universities in France.

Denis Gingras received Dr. Eng. EE, MS and PhD EE degrees from UBC/U Laval Canada and U Bochum Germany in 1980, 1984 and 1989. He was Post-doc STA Fellow at CRL, Tokyo Japan in 1989–1990, Director of the Digital and Optical Systems division at INO, Canada from 1990 to 1998, Guest Professor at INRIA, France in 1999 and is EECE Professor at the Université de Sherbrooke since 2000. He was Director of the Intelligent Materials and Systems Institute at UdeS from 2000 to 2007, Research Fellow at Observatoired de Paris in 2008 and Dean Associate for Research and Graduate Studies at the Faculty of Engineering of UdeS from 2009 to 2011.

1 Introduction

In the intelligent vehicle area, the determination of the current location is a fundamental task. There are many well-known low cost positioning systems for outdoor vehicles, most of them being GPS based. Unfortunately, GPS position estimates can be highly affected by errors due to satellite signals loss, multipath reflections (forests, tunnels and urban environments) or interferences. However, for active safety applications, it is imperative to guarantee accurate and robust vehicle position estimate at all time. Since positioning systems based on the GPS alone do not allow this, data fusion of proprioceptive (dead reckoning) and exteroceptive sensors is used to improve vehicle localisation (Dao et al., 2006).

26 D. Gruyer et al.

Recursive filtering is one of the most efficient approaches to solve real time applications based on state-space modelling. Although the basic Kalman filter suffers from poor performance in presence of model nonlinearities and non-Gaussianity, its extended version (EKF) has been up to now, the dominating state estimation technique used (Maybeck, 1982; Gutmann et al., 2001; Abuhadrous et al., 2003; Laneurit et al., 2003; Giroux et al., 2005). More recently, some new methods have been developed in order to improve estimation. They include some other variants of the Kalman filter such as the Divided Differences of first and second order (DD1 & DD2) (Ito and Xiong, 2000; Nørgaard et al., 2000) and the unscented Kalman filtering (UKF) (Julier and Uhlmann, 1996, 1997; Wan and Van Der Merwe, 2000). The principle of these new approaches is based on the linearisation of the process and measurement functions by using statistical linear regression functions, through sampling points in the region around the state estimate. Up to now, very little work related to performance comparison of those recent filters have been published. A study carried out in Lefebre et al. (2004) aims to evaluate some Kalman filter variants (EKF, DD1, DD2, UKF) capacities to linearise the process and measurement models. This work theoretically compares the filters’ performance separately for the predictive step and the correction step, but does not analyse their overall performances. Mourllion et al. (2005) describes performances of these estimators (EKF, DD1, DD2 and UKF) in their predictive steps for road vehicles localisation. The objective of this paper is to complement the work already carried out in Lefebre et al. (2004) and Mourllion et al. (2005) by taking into account the overall localisation process (both the predictive and corrective steps) in an experimental setup. We tackle only the positioning problem without feedback control. In order to ensure proper comparison between the methods, two criteria of performance have been defined and applied. The first criterion concerns the evaluation of the accuracy of each method. For this purpose, we use a centimetric RTK GPS as a reference trajectory. The second studied criterion is related to the consistency of the investigated methods using the Normalised Innovation Squared (NIS) metrics.

The paper is organised as follows: Section 2 provides an overview of recursive filtering approaches (EKF, UKF, DD1, DD2) for position estimation. Section 3 gives a description of the system modelling and the criteria used in order to evaluate filters’ performance. In Section 4 experimental results and analysis based on simulations are presented before the real data results are given in Section 5.

2 Review of Kalman filtering approaches for position estimation

2.1 Introduction

One of the main characteristics of Kalman filtering (Kalman, 1960) is its ability to provide estimation errors. Moreover, it remains optimal when the minimum variance criterion is considered and when the system is linear. When it is assumed that the sensors data are degraded by noise with known a priori distribution, the estimation problem is reduced to solving a system of stochastic equations. This one is based on a state space


model with two components: a process model (equation 1) and a measurement model (equation 2). Let f be the process function and h the measurement function, the estimation problem becomes,

1( , , )k k k kX f X u w (1)

( , )k k kZ h X v (2)

Xk represents the state vector prediction at time k and Zk is the measurement vector. uk is the input vector, wk and vk are respectively the process noise and the measurement noise at time k: (0, )k kw R and (0, ).k kv R The process model leads to predictive

equations, while the measurement update model is used in the corrective equations. The final estimation algorithm is done in a recursive predictive-corrective way. For nonlinear systems, the most popular recursive estimation method remains unquestionably the Extended Kalman filter described beneath.

2.2 The Extended Kalman Filter (EKF)

The EKF follows exactly the same scheme as the Kalman Filter. The main difference appears in the computation of the filter matrices. In the KF, the process and the measurement matrices are composed of “true” linear functions; whereas in the EKF, these matrices (called Jacobian matrices) are composed of Taylor first order linearised functions.

Although the EKF has been shown reliable in many practical driving situations, it has some well-known drawbacks. A major one concerns the hypothesis related to the point of linearisation. Theoretically, the nonlinear process function f is linearised around the true current state. But in the implementation, this function is linearised around the estimated value of X, leading to an additional error (Mourllion et al., 2005). Moreover, another obvious limitation of this filter relates to the difficulty of computing the Jacobian matrices. In complex systems, strong nonlinearities can generate system instability problems; therefore computation of these matrices can become unduly cumbersome in practice, for example when the process or measurement functions are non-differentiable. For intelligent vehicles applications however, the functions used are differentiable, so the main drawback concerns mainly the linearisation step.

To bypass some of these limitations, other derivative free methods are presented in the following sections. These methods have often shown themselves to be more powerful than the EKF filter and include UKF (Julier and Uhlmann, 1996, 1997; Wan and Van Der Merwe, 2000), DD1 & DD2 (Nørgaard et al., 2000).

2.3 The Unscented Kalman Filter (UKF)

The unscented transformation is a method for calculating the statistics of a random variable that undergoes a nonlinear transformation (Julier and Uhlmann, 1997). We consider a random propagating variable X through a nonlinear function, Y = f (X). A set of sigma points, deterministically chosen, with mean X (mean of X) and covariance Pxx (covariance of X) is computed. The nonlinear function is applied to each point to yield

28 D. Gruyer et al.

a cloud of transformed points with statistics Y and Pyy. The n-dimensional random variable X is approximated by 2n + 1 weighted points, which are used to calculate the statistics of Y as defined in the following equations. For 0 ,i n

0 0 ( )

1( ( ) )

2( )

1( ( ) )

2( )

i xx i i

i n xx i i n

X Wn

X n P Wn

X n P Wn

(3)

where , ( )xx in P is the i-th row or column of the matrix square root of

( ) xxn P (e.g. the lower triangular Cholesky factorisation). Wi is the weight of the i-th

point. The transformation is built following the steps below:

The sigma points are propagated through the nonlinear function. For 0, , 2i n

[ ]i if (4)

The mean is calculated as the weighted mean of the transformed points:

2

0

n

i ii

Y W

(5)

The covariance is obtained according to equation (6)

2

0

[ ][ ]n

Tyy i i i

i

P W Y Y

(6)

The UKF is an application of the unscented transform to a mean square recursive estimation (Julier and Uhlmann, 1996, 1997).

2.4 The divided differences Kalman filter of first and second order (DD1 & DD2)

These two filters were proposed by Nørgaard et al. (2000). Both are based on the Stirling interpolation, and their implementation methods are very similar. In the DD1, we are limited to the first order interpolation, whereas in the DD2, the functions are linearised at the second order. The DD1 & DD2 filters differ with the EKF from the fact that the Jacobian matrices are replaced by divided differences. Therefore, the correction steps are the same. The main difference appears in the filter covariance matrices update. In the EKF, we use a first order Taylor series development to linearise the function. In order to implement the Sterling interpolation, two operators are defined. Let us consider a one dimensional interval with length :

( )2 2

f X f X f X

(7)


1( )

2 2 2f X f X f X

(8)

The Stirling interpolation formula is obtained by applying both the above operators to ,X the mean value of X, rather than directly to X. The second order interpolation gives

2

( ) ( ) ( )( )

( )( )

2!

X DD X X

DD XX

f X f f X

fX

(9)

where

( ) ( )( )

2X X

DD X

f ff

(10)

2

( ) ( ) 2 ( )( ) X X X

DD X

f f ff

(11)

During the divided difference filters implementation, a Cholesky factor decomposition is introduced and used to compute the process and measurement noise covariance matrices, as well as the predicted and corrected state error matrices, through some characteristic divided differences matrices. These matrices are defined for both DD1 and DD2, and are presented in Nørgaard et al. (2000) and Lefebre et al. (2004).

3 System modelling and comparison criteria

3.1 Vehicle model and GPS model

All the compared filters are based on the kinematic model presented in Figure 1. The origin of the mobile frame is taken at the centre point of the rear axis. vlon and vlat

represent the longitudinal and lateral velocity, respectively. The prediction step equation is:

| 1 1| 1 1| 1

| 1 1| 1 1| 1

| 1 1| 1

cos( /2) cos( )

sin( /2) cos( )

k k k k k k k k k

k k k k k k k k k

k k k k k

x x V T T

y y V T T

T

where | 1 | 1 | 1 | 1[ , , ]k k k k k k k kX x y is the state of the system at the time k knowing the

state of the system at the previous time | 11. k kk X is computed in the current prediction

stage whereas 1| 1k kX is computed in the previous correction stage. [ , ]k kx y is the

position vector and k represent the yaw angle (heading). Consequently k is the yaw

rate. k lonV v is the vehicle velocity and T is the time period. k is the steering front

wheel angle which is used to take into account the kinematic constraints on the vehicle (Latombe, 1990; Kelly, 2000).

30 D. Gruyer et al.

The GPS observation model that we use is linear and is the same for all filters, with corresponding observation matrix:

100

010H (13)

The observation matrix is used in order to compute the predicted measurement from the predicted state:

| 1k k kZ H X (14)

Figure 1 Kinematic model (see online version for colours)

3.2 Comparison criteria

In order to evaluate and compare the different filters performances, many measures can be used. In this work, we focused on the accuracy and the consistency of the various filters.

3.2.1 Accuracy

The Root-Mean-Square Error (referred as RMSE), or the l2 norm, is by far the most popular measure of estimation accuracy and is given by,

1

1ˆ( )M

Ti i

i

RMSE X X XM

(15)


where M is a number of trials. X the estimation error ˆ ,X X X X is the true state

position sub-vector and X is its estimate. Another accuracy measure we consider in this paper is the average Euclidean Error AEE (Li and Zhao, 2001; Drummond et al., 1998) corresponding to the l1 norm and defined as

1

1ˆ( )M

Ti i

i

AEE X X XM

(16)

AEE and RMSE typically differs from the fact that the latter is an approximation of the error standard deviation, while AEE, more robust to outliers, approximates the mean absolute error, which is the average Euclidean distance between the real value and the estimate.

3.2.2 Filters’ uncertainty and consistency

In order to describe filters’ uncertainty, we consider the 2 envelopes. These envelopes are derived from the covariance matrix output of each filter. They are used to evaluate the level of confidence one can have in the estimates. Assume that output filter i has a position covariance submatrix Px,y given by,

2

, 2

x x y

x y

x y y

P

(17)

where i stands for the standard error for the filter on axis i. Then, assuming a Gaussian

distribution, the 2 envelopes cover the 95% probability region of the estimated error. A comparison between these envelopes and the real estimation error gives a performance assessment of the filter.

Similar to the 2 envelope measure, the 2 uncertainty ellipse areas are computed in order to evaluate each filter’s uncertainty. In fact, the results of the filters are given a posteriori with an uncertainty described by an ellipse. Before deriving their areas, we obtain the length of the main axis of these ellipses by computing the eigenvalues of the covariance matrix P. These eigenvalues are then weighted with a factor

21log(1 ),ak P where Pa is the membership probability (Smith and Cheeseman,

1986). The more filter uncertainty ellipse areas grow, the more its output is uncertain.

A state estimate X̂ with covariance matrix P is called consistent if it satisfies equations (18) and (19) (Lefebre et al., 2004; Bar-Shalom and Li, 1993).

ˆ ˆ[ ] [ ] 0E X X E X (18)

ˆ ˆ ˆ ˆ[( )( ) ] [ ]T TE X X X X E X X P (19)

Instead of using the true probability density function (generally not available during experiments), when the true state Xk is known, it is advised to use the Normalised Estimation Error Squared (NEES) (Bar-Shalom and Li, 1993) to evaluate the filter consistency

1nees Tk k k kX P X (20)

32 D. Gruyer et al.

A measure of consistency is found by examining the average NEES over M Monte Carlo runs of the filter. Under the hypothesis that the filters are consistent and approximately linear-Gaussian, nees

k is χ2 (chi-square) distributed with dim(X) degrees of freedom as M

tends toward infinity.

[ ] ( )neeskE dim X (21)

The validity of this hypothesis can be subjected to a χ2 acceptance test. Two cases arise:

1 In performing M Monte Carlo simulations, the average NEES is computed as

1

1 Mnees nees

k ikiM

(22)

2 When using real data with a single trial, the NEES measure is no more adapted (Bar-Shalom and Li, 1993). It is therefore recommended to use the Normalised Innovation Squared (NIS) measure, nis (Bar-Shalom and Li, 1993), to characterise the filters’ consistency.

1nis Tk k k kz S z (23)

where z is the filter innovation and S the innovation covariance matrix. Alike the NEES measure, under the hypothesis that the filter is consistent and approximately linear-Gaussian, nis is χ2 (chi-square) distributed with dim(Z) degrees of freedom. The average value of the NIS then tends toward the dimension of the observation vector ,[ ]GPS GPSZ x y which is 2.

4 Simulation results

4.1 Test scenario

Scenario 1 (see Table 1 with various tangential and normal accelerations) describes complex driving situations. A succession of longitudinal and lateral uniform accelerations is simulated and the corrective GPS data are assumed always available. The ground truth trajectories are generated by using a first order Euler discretisation of the generic continuous time curvilinear motion model from vehicle kinematics (Li and Jilkov, 2003):

The continuous form

( ) ( ) cos( ( ))

( ) ( )sin ( ( ))

( ) ( )

( )( )

( )

t

n

x t v t t

y t v t t

v t a t

a tt

v t

(24)


where (x; y), v, , at and an denote the mobile position (in Cartesian coordinates), speed, heading, tangential and normal acceleration, respectively. The compared filters do not use a continuous but a discrete form of the vehicle’s model. This discrete form (equation 25) is obtained from the continuous form (equation 24).

The discrete form

1

1

1

1

cos( )

sin( )k

k

k k

k

k

k k k k x

k k k k y

k k t v

n

k kk

x x Tv

y y Tv

v v Ta

aT

v

(25)

k is the discretisation noise. The sampling period is T and k is the time index. For each

Monte Carlo run 1, 2, , ,i M

0 0

0 0

( ) 2 ( ) 20 0 0 0

( ) 2 ( ) 20 0 0 0

( ) 2 ( ) 2

( ) 2 ( ) 2

( , ), ( , ),

( , ), ( , )

(0, ), (0, ),

(0, ), (0, ),

k x k y

k v k y

i ix y

i iv

i ix y

i iv

x x y y

v v

Table 1 Scenario 1 parameters

Time(s) 2(m.s )kt

a 2(m.s )kna

0.0–20 1.0 0.0

20.1–30 0.0 1.5

30.1–160 0.0 0.0

160.1–180 0.0 –1.5

180.1–200 –1.0 0.0

During our simulations, we chose:

0

0

0 0 0 0

0

30

3 3 4

10 ; 10 ;

1 / , 0.01 / ;

/ 4 ; 10

10 ; 10 / ; 10x y v

x y

v

x y m cm

v m s m s

rad rad

m m s rad

Figure 2 shows the quiver plots (velocity vector) with sampling period 0.1 s. The simulated mean velocity is presented on Figure 3. Two high dynamic change areas are identified at the beginning and at the end of the tested trajectory (see Figure 2, rectangular coloured areas).

34 D. Gruyer et al.

Figure 2 Ground truth trajectory, scenario 1 (see online version for colours)

Figure 3 Ground speed, scenario 1 (see online version for colours)


4.2 Accuracy

Figure 4 shows the filters’ Average Euclidean Error in the first scenario after 20 Monte Carlo runs. All the filters behave similarly, in particular when the vehicle dynamic is low. Main differences appear when the vehicle undergoes high dynamic (rectangular coloured areas in Figure 2). In such areas the position error grows considerably for each filter. When we focus on the circled areas of Figure 4 (high dynamic areas), we can notice that second order filters (UKF and DD2) have a slightly better accuracy, compared to first order filters (EKF and DD1).

Figure 4 Average Euclidean Error in scenario 1 (see online version for colours)

This is confirmed by Table 2; the mean error computed on high dynamic areas is higher than the overall mean error and the last two filters (DD2 and UKF) are more accurate than the first order filters (DD1 and EKF). In fact, the scenario starts with important longitudinal dynamics, followed by lateral one, which are better handled by second order filters in presence of GPS data. This result points toward a better global accuracy for second order filters.

Table 2 Mean and max AEE for scenario 1

Test track scenario 1 High dynamic areas Method

Mean AEE (m) Max AEE (m) Mean AEE (m) Max AEE (m)

EKF 1.43 2.48 2.05 2.48

DD1 1.43 2.49 2.07 2.49

DD2 1.31 2.20 1.97 2.20

UKF 1.31 2.20 1.97 2.20

4.3 Filters’ uncertainties

Investigated filters’ uncertainties are characterised by their 2 envelopes. Figures 5 and 6 illustrate these envelopes for all filters in both X and Y axis. Filters’ envelopes on each axis are almost the same, and there are no visible differences during the test. On both

36 D. Gruyer et al.

axis, position errors are almost always inside the envelopes. Only few error points are found outside the 2 envelopes within the high vehicle dynamics region (see circled areas). In these regions, lateral acceleration occurs when vehicle speed is important (21 m/s). This behaviour translates into an important nonlinearity which is slightly better handled by second order filters. Consequently, UKF and DD2 Y-axis errors are more likely to remain inside their envelopes than EKF or DD1 errors do. Hence, higher order filters have slightly better performances in such scenario.

Figure 5 Average X-axis uncertainty envelopes and errors in scenario 1 (see online version for colours)

Figure 6 Average Y-axis uncertainty envelopes and errors in scenario 1 (see online version for colours)


5 Results based on real measurements

5.1 Test track and collected data

The tests carried out in subsequent scenarios 2 and 3 use real data collected with one of our experimental vehicles moving at 15 m.s–1 (average) on the Satory test track in Versailles, France. The complete track has a length of approximately 5.5 km. The test track is made up of important curves as well as straight lines. It includes an open road and forest areas with strong turns. It is assumed that cross slopes and super-elevation angles remain negligible along the track. The experimental sensor data, used in equation (12), were provided by an inertial measurement unit Crossbow VG400 (yaw rate ), an odometer fixed to the front axis (vehicle speed vlon) and an encoder that

recorded the steering angle at the front wheel (). A low cost GPS receiver provided correction data. During the tests, the reference trajectory (ground truth) was obtained using a RTK1 GPS (Thales) with a precision close to 10–2 m. The initialisation of the four investigated filters was made with the low cost GPS measurements.

The state noise was derived from the various sensor noise specifications. In our experiment, we had 0.005m/sodometer and 0.05 rad/s.gyro

During scenario 2 (complete test track, see Figure 7), all sensors were synchronised thanks to GPS timestamps, so that the system provided an output rate of 5 Hz. In areas surrounded by trees (see Figure 8 around (400, –500)) the low cost GPS receiver yields an error of more than 10 m compared to the reference.

Scenario 3 took place on part of the test track described above. In this scenario, the low cost GPS receiver was switched off from time to time (see Figure 9). During GPS blackouts, positioning was performed using only proprioceptive sensors while the filters ran in the predictive mode only.

Figure 7 Test track in scenario 2 (see online version for colours)

38 D. Gruyer et al.

Figure 8 Zoom on forest area in scenario 2 (see online version for colours)

Figure 9 Filters localisation in scenario 3 (see online version for colours)


5.2 Accuracy

5.2.1 Scenario 2

Figures 10 to 11 present positioning Euclidean errors for the four filters investigated. The methods show very similar results. Table 3 presents the mean and maximum values of the Euclidean errors. Results confirm what Figure 10 indicates. The performances of the EKF, UKF, DD1 and DD2 filters are globally very similar (2.91% difference). When we zoom in on these results, we can notice that EKF and DD1 have almost exactly the same mean error 3.093 m. They appear globally more accurate than UKF and DD2

Figure 10 Euclidean positioning errors in scenario 2 (see online version for colours)

Figure 11 AEE zoom on forest area in scenario 2 (see online version for colours)

40 D. Gruyer et al.

Table 3 Mean and max AEE in scenario 2

Method Mean AEE (m) Max. AEE (m)

EKF 3.093 15.815

UKF 3.186 14.732

DD1 3.093 15.815

DD2 3.185 14.732

Note: Mean AEE 3.186 m and 3.185 m.

However, the maximum error values, which are reached in the region from 170 s to 210 s in Figure 11, reveal that the UKF and DD2 locally exhibit a better behaviour. The reason for this is due to the fact that this area is characterised by poor quality GPS data combined with strong nonlinearities in the prediction mode (strong turns in the trajectory). UKF and DD2 better handle such nonlinearities and are less affected while updating GPS data. Table 3 shows that the second order filters (UKF and DD2) are slightly more accurate (max AEE 14.732 m) than first order filters (EKF and DD1, max AEE 15.815 m) in such a situation. But overall, the filters behave very similarly.

5.2.2 Scenario 3

The Euclidean positioning error for this third scenario is shown in Figure 12. We can see that in presence of GPS signal, all methods show a rather comparable performance (like in scenario 2). Important differences appear however, when the GPS is switched off. This is an interesting illustration of the theory underlying the design of these filters, which is different in the manner they handle the nonlinear process function. These theoretical differences are presented in Lefebre et al. (2004) and Mourllion et al. (2005).

Figure 12 Euclidean positioning error in scenario 3 (see online version for colours)

In this scenario, the common GPS observation model is linear. Thus, very little differences will be observed between those filters during connection mode. However, during GPS outages, Figure 12 confirms that EKF and DD1 (first order) filters react


similarly and are more accurate than UKF and DD2, which are also similar. During the first GPS outage, EKF and DD1 mean AEE (4.89 m and 4.73 m) is almost 1 m lower than DD2 and UKF mean AEE (5.73 m and 5.75 m). The maximum AEEs show the same differences (see Table 4). During the second GPS outage, the AEE differences are reduced to about 15 cm for the mean values and 50 cm for the maximum values. In presence of path curvatures, DD2 and UKF react as if the vehicle turns earlier than it does in reality. In fact, the bias between the first and second order filters originates from the way the estimate is being computed. Using velocity and heading, the primary computation is done in polar coordinates and the result is mapped back into the Cartesian space. This polar to Cartesian conversion problem was handled in Mourllion et al. (2005), and it leads to a cumulative error for second order filters.

Table 4 Mean and max AEE during GPS outages (scenario 3)

GPS outage 1 [10 – 75 s] GPS outage 2 [110 – 135 s]

Method meanAEE (m) maxAEE (m) meanAEE (m) maxAEE (m)

EKF 4.89 11.95 4.87 19.53

DD1 4.73 11.56 4.88 19.59

DD2 5.73 13.05 5.02 20.08

UKF 5.75 13.00 5.02 20.08

5.3 Consistency

Normalised Innovation Squared (NIS) expresses filter consistency (Bar-Shalom and Li, 1993). Figure 13 shows the NIS for the position innovation normalised to a 95% probability region, assuming a 2 distribution. Most of the time the NIS for the EKF, UKF, DD1 and DD2 filters is below 2.0. This means that estimated filter uncertainties are most of the time consistent with the true estimation error, given a 95% probability region. An exception appears in the forest region (from 170 s to 210 s), where NIS values are often above 2.0. The inconsistencies are due to repeated large biased GPS measurements in the forest region (Lambert et al., 2009). Even in this region, the four estimators behave similarly. Further experiments led to similar results.

Figure 13 Filters NIS in scenario 2 (see online version for colours)

42 D. Gruyer et al.

6 Conclusion

In this paper, an experimental comparative study of four Kalman based data fusion filters (EKF, UKF, DD1, and DD2 filters) was presented. Previous works (Lefebre et al., 2004; Mourllion et al., 2005) exhibit major differences between those filters. Lefebre et al. (2004) shows theoretical differences whereas Mourllion et al. (2005) analyses practical differences during the prediction steps. Nevertheless, practical experiments taking into account the whole positioning process (both predictive and corrective steps) exhibit minor differences. The differences observed during the prediction step in terms of accuracy and uncertainty (due to linearisation of the distribution or the use of sigma points) are strongly reduced during the corrective step.

According to our experiments, the choice of a given filter is not obvious and depends either on the presence of GPS corrective data or strong nonlinearities on the trajectory:

if the GPS signal is unavailable for a long time the EKF or the DD1 are recommended as mentioned in Mourllion et al. (2005).

if the GPS signal is available (i.e. if the correction step runs efficiently), then the use of one or another among the presented filters does not bring any significant improvement (in terms of accuracy or confidence interval of the position estimates). Nevertheless, in presence of strong nonlinearities of the prediction step, UKF or DD2 give slightly better results than EKF and DD1.

In further work, these filters shall be compared to other estimators which are theoretically known to be more robust, such as multiple model filters (Ndjeng et al., 2011).

References

Abuhadrous, I., Nashashibi, F. and Laurgeau, C. (2003) ‘3-D land vehicle localization: a real-time multi-sensor data fusion approach using rtmaps’, International Conference on Advanced Robotics, Coimbra, Portugal, pp.71–76.

Bar-Shalom, Y. and Li, X.R. (1998) Estimation and Tracking: Principles, Techniques, and Software, Artech House, Boston, MA.

Dao, T.S., Leung, K.Y.K., Clark, C.M. and Huissoon, J.P. (2006) ‘Cooperative lane-level positioning using markov localization’, IEEE Intelligent Transportation Systems Conference, Toronto, Canada, pp.1006–1011.

Drummond, O.E., Li, X.R. and He, C. (1998) ‘Comparison of various static multiple-model estimation algorithms’, SPIE Conference on Signal and Data Processing of Small Targets, pp.510–527.

Giroux, R., Gourdeau, R. and Landry, R. (2005) ‘Extended Kalman filter implementation for low-cost ins/gps integration in a fast prototyping environment’, Symposium on Navigation of the Canadian Navigation Society, Toronto, Canada, pp.1–11.

Gutmann, J.S., Weigel, T. and Nebel, B. (2001) ‘A fast, accurate, and robust method for self-localization in polygonal environments using laser range finders’, Advanced Robotics Journal, Vol. 14, No. 8, pp.651–668.

Ito, K. and Xiong, K. (2000) ‘Gaussian filters for nonlinear filtering problems’, IEEE Transaction on Automatic Control, Vol. 45, No. 5, pp.910–927.

Julier, S.J. and Uhlmann, J.K. (1996) A general method for approximating nonlinear transformation of probability distribution, Technical report, University of Oxford.


Julier, S.J. and Uhlmann, J.K. (1997) ‘A new extension of the Kalman filter to nonlinear systems’, International Symposium on Aerospace/Defense Sensing, Simulation and Controls, Orlando, Florida, pp.182–193.

Kalman, R.E. (1960) ‘A new approach to linear filtering and prediction system’, Transactions of the ASME-Journal of Basic Engineering’, Vol. 82(D), pp.35–45.

Kelly, A. (2000) Some useful results for closed-form propagation of error in vehicle odometry, Cmu-ri-tr-00-20, Robotics Institute, Carnegie Mellon University.

Lambert, A., Gruyer, D., Vincke, B. and Seignez, E. (2009) ‘Consistent outdoor vehicle localization by bounded-error state estimation’, IEEE/RSJ International Conference on Intelligent Robots and Systems, Best Paper Award Finalist, pp.1211–1216.

Laneurit, J., Blanc, C., Chapuis, R. and Trassoudaine, L. (2003) ‘Multisensorial data fusion for global vehicle and obstacles absolute positioning’, IEEE Intelligent Vehicles Symposium, Columbus, OH, USA, pp.138–143.

Latombe, J.C. (1990) Robot Motion Planning, Kluwer Academic Publishers.

Lefebre, T., Bruyninckx, H. and De Schutter, J. (2004) ‘Kalman filters for non-linear systems: a comparison of performances’, International Journal of Control, Vol. 77, No. 7, pp.639–653.

Li, X.R. and Jilkov, V.P. (2003) ‘Survey of maneuvering target tracking. part 1: Dynamic models’, IEEE Transactions on Aerospace and Electronic Systems, Vol. 39, No. 4, pp.1333–1364.

Li, X.R. and Zhao, Z. (2001) ‘Practical measures for performance evaluation of estimators and filters’, Workshop on Estimation, Tracking, and Fusion – a Tribute to Yaakov Bar-Shalom, Monterey, CA, USA, pp.467–480.

Maybeck, P.S. (1982) Stochastic Models, Estimation and Control, Academic Press, New York.

Mourllion, B., Gruyer, D., Lambert, A. and Glaser, S. (2005) ‘Kalman filters predictive steps comparison for vehicle localization’, IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.565–571.

Ndjeng, A., Gruyer, D., Glaser, S. and Lambert, A. (2011) ‘Low cost imu-odometer-gps ego localization with imm for unusual maneuvers’, Information Fusion, Vol. 12, No. 4, pp.264–274.

Nørgaard, M., Poulsen, N.K. and Ravn, O. (2000) Advances in derivative-free state estimation for nonlinear systems, Technical Report IMM-REP-1998-15, Technical University of Denmark.

Smith, R.C. and Cheeseman, P. (1986) ‘On the representation and estimation of spatial uncertainty’, International Journal of Robotic Research, Vol. 5, No. 4, pp.56–67.

Wan, E.A. and Van Der Merwe, R. (2000) ‘The unscented Kalman filter for nonlinear estimation’, Adaptive Systems for Signal Processing, Communication and Control Symposium, Lake Louise, AB, Canada, pp.153–158.

dominique gruyer and alain lambert* mathias perrollaz denis … · 2014. 1. 30. · the covariance...

Documents