Robust Positioning in Safety Applicationsfor the CVIS Project

Dominique GRUYER, Steve PECHBERTI, Denis GINGRAS and Francis DUPIN

Abstract— This paper describes hybrid fusion module usedin a strong localization context (POMA) for embedded vehicleapplications. This work has been developed in order to give ananswer to the POMA (Positioning, Maps and local referencing)sub project objectives. These objectives are to provide, for a setof high level applications, a positioning service included a servicequality, a metric accuracy (lane) and a robust result. This workis involved in CVIS European project. The use of IMM approachin the Hybrid Fusion module will be justified in comparison tothe different current probabilistic methods. The IMM, contraryto the non modular methods, is based on the discretization of thevehicle evolution space into simple maneuvers, represented eachby a simple dynamic model such as constant velocity or constantturning etc. This allows the method to be optimized for highlydynamic vehicles. The application of this positioning servicewill be presented in a real time embedded architecture. Thepresented results are based on real measurements collected fromrepresentatives scenarios (test track, peri-urban road, highway).These results show a real interest in using the new IMM methodin order to reach the POMA’s objectives.

I. INTRODUCTION

Until now vehicle positioning has been mainly used innavigation systems for information delivery applications Inrecent years however, various positioning-based safety sys-tems have been developed For these applications, positionestimation must be much more robust and reliable. To achievethis goal, a new positioning module called POMA, willbe presented and described in details. For this kind ofapplications, positioning is usually based on ”‘hybrid fusion”’algorithms. In this paper, we will review briefly an improvedIMM (Interacting Multiple Model) which will be furtherdescribed in details. The IMM algorithm is a well knownmodular approach, which is based on the discretization of thevehicle evolution space into simple methods to be optimizedfor highly dynamic systems such as vehicles. Unfortunatelyclassical IMM shows some drawbacks concerning some realtime multi sensors applications. In this work, we focus on

Dominique GRUYER is a Researcher at the Laboratoiredes Interactions Vehicules Infrastructure Conducteur (LIVIC -LCPC/INRETS), 14, Route de la miniere, 78000 VERSAILLES,dominique.gruyer@inrets.fr

Steve PECHBERTI is a Research Engineer at the Laboratoiredes Interactions Vehicules Infrastructure Conducteur (LIVIC -LCPC/INRETS), 14, Route de la miniere, 78000 VERSAILLES,steve.pechberti@inrets.fr

Denis GINGRAS is Professor Dr Eng. at the Universit de Sherbrooke,Sherbrooke, Qc, Canada, denis.gingras@usherbrooke.ca

Francis DUPIN is a Research Engineer at the Laboratoire des InteractionsVehicules Infrastructure Conducteur (LIVIC - LCPC/INRETS), 14, Route dela miniere, 78000 VERSAILLES, francis.dupin@inrets.fr

vehicle positioning with asynchronous sensors in order toreport these drawbacks and then propose a new solution.The algorithm developed in this paper is embedded in thePOMA platform in order to provide a positioning data. Thedeveloped architecture (Hardware and software) have beenmade in order to give an answer to the constraint of laneaccuracy and robustness of the CVIS European project. Inthe following section, a state of art is presented in order tojustify the use of IMM approach in POMA project. ThenIMM will by briefly presented. Moreover, the adaptationand improvement of this method for POMA project willbe presented. These improvements are mainly done in orderto get a more adapted set of used models and to allow anefficient likelihood updating. Section III will present bothreal hardware and software architecture developed speciallyfor POMA project and real time use. Section IV will showthe results, the performances and the limits of this hybridfusion on several situations. The tests will be done in theSatory’s test track, in the Minire’s peri-urban area and theA86 highway in south of Paris.

II. DATA FUSION FOR ROBUST POSITIONNING SYSTEM

A. The state of the artIn this section, we describe the different robust positioning

methods usually applied to driver assistance systems[5].1) Kalman Filter: The most popular approach for vehicle

robust positioning is based on the Kalman filter (KF) [11]. Inthis case, sensors data considered imperfect and vague can bemodeled as a random noise distribution known a priori. Thedata fusion is reduced to solving a system of state equations.For the standard Kalman Filter, the system is composed onlyof linear equations, its resolution being optimal in a statisticalsense (minimum variance of the estimation error).

2) Extended Kalman Filter: In a real situation however,the behavior of a vehicle is not linear. To solve the cor-responding nonlinear problem, an extension of the Kalmanfilter is often applied, known as the Extended Kalman filter(EKF). It is based on a linearization of the equation systemand assumes that all variations are small enough to insurethe validity of the linear hypothesis and estimation. Depend-ing on the acquisition frequency, however, the linearizationcan generate distorted estimation, and in this case, provideresults which are corrupted or unreliable information. It hasbeen found that in practice, without exteroceptive data, theprediction is correct for short periods of time, depending onthe complexity of the vehicle’s movement. [4]

2010 IEEE Intelligent Vehicles SymposiumUniversity of California, San Diego, CA, USAJune 21-24, 2010

TuE1.3

978-1-4244-7868-2/10/$26.00 ©2010 IEEE 262

3) Unscented Kalman Filter: Recently, Julier andUhlmann have introduced an approach called the UnscentedKalman Filter (UKF) [9] [8] [10]. It is based on the ScaledUnscented Transformation for the computation of randomdata. This approach has been used to manage non linearcases which cannot be linearized by the EKF. Due to themany hypotheses implied however, it is not well adapted tovehicles positioning having a quick dynamical behavior [16][17].

4) DD1 and DD2 algorithms: Nørgaard and al [20]proposed two closely related algorithms, namely Divided Dif-ferences of order 1 (DD1) and Divided Differences of order 2(DD2). These algorithms introduce a set of estimators basedon a polynomial approximation of the non linear functionobtained by an extension of the Stirling interpolation formula.Conceptually, the principle of these algorithms is the same asthe EKF one, but the implementation is different. Contraryto the Taylor approximation, no derivative is requested forthe interpolation formula. The implementation is much easierand can provide an estimator even on a singularity where noderivatives are defined. It should be pointed out that the DD2has the same spatial latency problems as the UKF. Only theDD1 algorithm provides results similar to the EKF with betterprecision on the position estimate.

5) Monte Carlo localization and particle filters: In orderto better take into account the systems non-linearities, anothermethod based on points clouds has been proposed by J.E.Handschin [7], called the Monte Carlo Localization. It usesa random set of sample points on the underlying probabilitydensity function. It doesn’t attempt to describe the densityfunction exactly, but rather provides a coarse representationof it by keeping some random sample points (called particles)which evolve thanks to different algorithms [12] [13] [6].This approach is powerful because it can be used to achieveglobal positioning, solve multiple positioning problems ormodel sensor noise. The more we have particles, the betterthe results are. The main issue by using this approach isto obtain a proper trade-off between quality of the resultsand computation time which is proportional to the numberof particules used.

6) The Interacting Multiple Model: The approach de-scribed here makes the hypothesis that according to a finiteset of prototypical behaviors of a given complex systems,its modeling can be achieved by decomposing it into severalsub-systems, which are less complex and easier to implement.So each of these sub system will emulate the behavior of theglobal system for a specific situation. The complementaritiesand redundancies of the sub systems can, after a fusionstep, cover the entire functional space of the global system.This approach called Interacting Multiple Model (IMM), hasbeen developed by Blom and Bar Shalom [1] [2]. It iswidely used for estimation, control, system modeling, objecttracking with highly nonlinear dynamic, and recently forvehicle positioning. In this case, the method is robust andprovides a good position estimate for the various typical

vehicle movement [18]. In its original form, however, theIMM filter is sensitive to the use of corrupted or noisy data,which can cause it to fail. That’s why the IMM used in thiswork is a modified version with new strategies presented inthe following section.

B. Hybrid Fusion for robust positioning

In order achieve the objective to assess an accurate androbust positioning, sensors are needed. Theses sensors areshared in 2 groups. The first group involves the exteroceptiveand reference sensors (GPS). The second group correspondsto the prorioceptive sensors (INS, Odometer ). Figure 1presents this hardware sensors architecture of the POMAproject. Figure 2 shows the functional architecture includedthe hybrid fusion module.

Figures 1 and 2 presents the hardware and functionalarchitectures of the POMA module for vehicle positioning.

Fig. 1. POMA Main architecture

Fig. 2. POMA Functional architecture

1) IMM Principles: The proposed IMM formulation usedhere is based on the hypothesis that the vehicular system ofequations describes a Markov process [1]. In that case, thestate of the chain represents, for a given time range, a vehiclebehavioral mode (or model) of the vehicular system (chosenamong a given set of available models or behavioral modes).The markovian process is characterized by its probability

263

transition matrix which is assumed known a priori. An IMMestimator is used to predict the current behavioral mode ofthe vehicular system. For a given IMM estimator, a set ofequations is used to describe each of the different modelsdefined [2]. The Markov model probabilities are updated ateach measurement, and the computed weighting factors areused to determine the next model. So the observed systemevolves by selecting one model out of r defined models witha switch between models managed by a Markov chain givenby the matrix

∏. We consider that r models describe all

behaviors of the system. The model j is defined by the stateequations :

{x(k) = Fj(k)x(k − 1) + Gj(k)u(k − 1) + rj(k − 1)

y(k) = Hj(k)x(k) + qj(k)(1)

where x(k) is the state vector of the vehicular sys-tem at the instant t, Fj(k) is the evolution matrixof the system state during the event noted M j(k) ={the model j is active at the time k}. u(k) is the vector ofcommand at the time k, Gj(k) is the command matrix, Hj(k)is the measurement matrix, rj(k) and qj(k) are respectivelythe state noise and the measurement noise. These noisesare Gaussian, white, mutually independent, with zero meanand with covariance matrix Rj(k) and Qj(k). y(k) is themeasurement vector of the system at time k. i, j ∈ 1, ..., rare indices of the N models.

The switch between models are governed by a Markovchain with a limited number of states and where the proba-bility to switch from M i(k − 1) to M j(k)

pij = Pr{M j(k) | M i(k − 1)

}(2)

The r2 probabilities pij are supposed known. If it is notthe case, we can set them to 1

r2 at the initial state [2].The IMM estimator is a recursive process which can be

splitted in multiple steps at each iteration, the aim is to obtainthe state of the vehicular system and its associated covariancematrix.

x(k | k) = E(x(k) | yk−1)P (k | k) = E(x(k)x(k)T | yk−1)

(3)

where x(k) = x(k) − x(k | k) and yk = y(1), ..., y(k).For that, we use different filters, which produces differentestimations, one for each local model. Then we use theseestimations to compute the residual errors. These residualsare used to compute the probability of activation for eachmodel by using a likelihood function. The global estimationof the system state is then computed as a weighting of theestimations coming from the different models filters.

The figure below shows the structure of the IMM algorithmfor two models. At each iteration, the IMM algorithm exe-cutes four steps to provide a global estimation of the vehicularsystem state.

The four steps at each iteration are :

Fig. 3. IMM structure for 2 models

• The interaction and estimation mixingFrom the estimation xi(k − 1 | k − 1) given for eachmodel and the probability ui(k− 1) computed for eachmodel at the previous stage, the mixed estimation ˆxi(k−1 | k−1) is obtain. The mixed covariance matrix Pi(k−1 | k − 1) is provided too.

• The filtering and the likelihood computationThe Kalman filter is used in order to update both thesystem state and its dedicated covariance matrix xi(k |k), Pi(k | k).

• Activation probabilities updateThe likelihood function is used in order to update theactivation probability µi(k).

• global estimationThis stage allows to merge the estimations provided byeach model xi(k | k). This stage has for main goal toevaluate each filter and to merge their outputs in orderto compute a new global estimation x(k | k) and a newcovariance matrix P (k | k). The current fusion functionconsists to make the sum of every estimations weightedby their respective probability.

More details on this filter implementation can be found in[18], [14] and [15].

2) Application to robust vehicle positioning: In order torepresent the various system maneuvers, we used three simplemodels to describe the vehicle evolution: the constant velocityCV, the constant acceleration CA, the constant turning CT.Details for the first three models (CV, CA, and CT) are givenin [18] and [14].

3) IMM functionalities improvement for POMA service:In order to adapt the classical IMM approach to the position-ing problem, it is necessary to take into account the differentvehicle manoeuvres. For this reason, 2 new models have beenadded to the existing ones. The final set of models usedin the POMA’s Hybrid Fusion module are the CV, the CA,the CT, the CS (Constant Stop) and the CR (Constant Rear)for backwards driving situation. Moreover, the data providedby GPS and proprioceptive sensors are asynchronous. So,now, the stage of likelihood updating is not adapted to theconstraint of POMA service accuracy. In order to solve

264

Fig. 4. positioning and model probabilities with standard IMM approach

this problem, an innovative approach has been implementedin order to update likelihood both in correction stage andprediction stage. With this mechanism, the identification ofthe current manoeuvre is easier. Effectively, the probabilityapplied to each model is now available every time. In orderto shows the efficiency of these improvements, a simplescenario is presented. Two sets of results are given (Figure4 and 5). These results present the positioning and theprobabilities for the different models (maneuver). The usedmodels are Constant Velocity model (CV) for the free motionmode, Constant Acceleration model (CA) that describe thelongitudinal dynamics, Constant Turn model (CT) for lateraldynamics, Constant Stop model (CS) for no motion andConstant Rear model (CR) for backwards driving situation.The higher the probability is, the more confident the systemis in the predicted active mode.

Fig. 5. positioning and model probabilities with new IMM approach

Figure 4 shows a bad behaviour of the filter when, duringa loss of GPS data, the navigation path involving severaldifferent manoeuvres (i.e. straight and turn manoeuvres).Of course, with asynchronous data and without likelihoodupdating during the prediction stage, the filter diverges.The probability curves shows a CV manoeuvre selection(time steps 100 to 170) even when the vehicle make a turnmanoeuvre. Figure 5 presents the result with the new IMMalgorithm and a comparison between the reference track, therecorded GPS positions and the outputs of the Hybrid fusion.We could notice that even an important GPS loss appears, theIMM computation is still robust regarding the reference track.During the GPS loses, models probabilities are not update andthe IMM algorithm is only feed with proprioceptive sensorsdata. The details of this new implementation of the IMMfilter and explanation of the improvements are given in [19].

III. POSITIONNING MODULE ARCHITECTURE (POMA)

A. Embedded Hardware implementation

In order to test and to evaluate the POMA architecture andthe positioning service, a demonstrator has been designed.The embedded hardware was composed by a PC running thePOMA system equipped with a QFree CVIS PCI card. Onthis card, the following sensors are available: a GPS receiver,an accelerometer and a WAVE module. Figure 6 illustratesthe material used to validate the demonstrator

Fig. 6. hardware embedded architecture

1) Software implementation: The POMA’s software archi-tecture was build using RTMaps platform[3]. RTMaps allowsto record data flow, to replay the data and to prototypeapplications. The Hybrid Fusion package consists of threelevels of components (Figure 7). The first level (interfaces)allows processing the data coming from the sensors. Theseinterface modules give a set of functionalities for data de-noising, bias estimation, data formatting. The second levelprovides multiplexors for each sensor type. The last levelconcerns the hybrid fusion algorithms. Two fusion algorithmsare provided: an extended Kalman filter and the new IMMfilter.

This way to implement application is really efficient be-cause at every time, it is possible to tune and to change everyparameter of the RTMaps components. In fact, each compo-nent (interface, multiplexor, hybrid fusion, result display) hasan interface of properties.

265

Fig. 7. modular architecture for hybrid fusion

Fig. 8. POMA’s service in CVIS platform

Figure 8 present the final CVIS’s platform with thePOMA’s platform. In this figure, in same time POMA’scomponent provide vehicle positioning and mapmatchingservices.

IV. INTERPRETATION AND PERFORMANCE ANALYSIS

A. Test scenarios and results

This section will present the Hybrid Fusion results withthe POMA platform. Three types of road have been tested.The two first scenario deal with the Satory’s test track andthe Miniere’s peri-urban road.

The third scenario corresponding to highway situation.This last scenario is very interesting because of the severaltunnels and area disturbing the GPS data reception.

B. Results on the Satory’s test track

The first site where the POMA module has been tested isthe Satory test track. A vehicle equipped with the POMAmodule drove around the test track. Figure 9 shows the com-parative performance of different hybrid fusion algorithmscompared to the reference test track and the GPS positionestimate. For the figures 9, 12 and 16, the same color codeis used: that is the GPS position in blue, the EKF positionestimate in red, the result of the IMM in green and thereference test track position in cyan.

Figure 10 presents the time period of the EGNOS receivedsignal, we can observe that EGNOS signal is not as stableas we could expect. A lot of data losses are visible.

Fig. 9. trajectory reference on the Satory’s test track

Fig. 10. time period of the EGNOS sensor

Figure 11 presents the error between the Hybrid Fusionoutput and the reference. We can notice that the maximumerror observed is less than 5 meters with an average value of 2meters. These results are obtained with a low cost configura-tion (low cost sensors). It is an interesting result because thisaverage value corresponds to the POMA accuracy constraint.This accuracy is lower than the width of a lane (3.5 m).

Fig. 11. positioning error (in meter)

1) Results on the Miniere’s peri-urban road: The next testis performed on the Miniere’s road. For this test session,a number of areas had no signal from the sensor EGNOS.This was mainly due to the forest zone. This session is veryinteresting in order to evaluate the divergence of the filterwhen no GPS data are available to feed the correction stageof the hybrid fusion. Figure 12 presents the different hybridfusion outputs compared with GPS impacts and the test trackreference. We can see the 2 critical forest areas where no GPSdata are available (Figure 15).

The second test has been performed on the Miniere road.For this test, we have simulated the loss of the EGNOS signal

266

in order to determine the drift of the system when this one isonly fed with a low-cost GPS. Figure 12 shows the differentcomputation outputs compared to the GPS position and thetest track reference position.

Fig. 12. reference trajectory on the Minire’s peri-urban road.

Figure 13 shows three important losses of the EGNOS datafor a long time, more than 30 seconds.

Fig. 13. time period for EGNOS sensor

About the positioning error (Figure 14), we can observethat the maximum error for this peri-urban scenario is lessthan 12 meters after a 30 second lose of EGNOS signal,even if this error is higher than in optimal condition, thiserror is lower than the GPS inaccuracy. In this case, EGNOSsensor provides a bad positioning (20 meters of inaccuracy)due to a forest area. During the EGNOS unavailability, thedivergences are due to the inertial sensor bias and noise.So, in future works, an effort will be done on this biasand noise assessment in order to improve the quality ofproprioceptive data. Moreover the mechanism used for thelikelihood updating in the prediction stage is very usefulfor the proprioceptive failure identification and hybrid fusionauto diagnosis stage.

Fig. 14. positioning results for Minire’s peri-urban road

Fig. 15. loss of GPS data in forest area

Fig. 16. reference trajectory for highway (A86)

2) Results on the A86 highway: The last test sessiontakes place on a section of the A86 highway. This sectionis interesting because many sections of this highway havetunnels. Figure 16 presents the bird view of route (half ahour) on the A86 highway. Figure 17 shows the different areaproducing disturbances in the EGNOS data availability (lossof data, low integrity, and bad accuracy). These critical areascorrespond to the tunnels locations. These figures also showthe error between the reference, the GPS impacts and hybridfusion outputs. Different interesting situations are displayedon these graphics. The first interesting point to notice is onthe graph 1. We can observe a discrepancy between the hybridfusion outputs and the reference, but if you look at Figure 18,at the same time we can observe that the EGNOS confidenceis very good whereas the results differ, it is due to the fact thatalthough confidence in the system EGNOS at this momentis correct, the values provided by EGNOS are corrupted. Forthe graph 2 and 3 of the figure 17, even if we get an EGNOSloss, the positioning is still robust with an error always lowerthan 15 meters which is better than some results obtainedwith a low cost GPS in good condition. Here we must keepin mind that these results are obtained in tunnel situation.

Fig. 17. loss of GPS or bad behavior of GPS data for highway (A86)

267

Fig. 18. time period for EGNOS sensor

Fig. 19. positioning results for highway area

V. CONCLUSION AND FUTURE WORKS

This paper has presented a embedded positioning systemdeveloped for safety vehicular application in the EuropeanCVIS project. Different hybrid fusion approaches have beencompared for the positioning issue. One method has beenselected for its robustness, the Interacting Multiple Modelsolution. To this solution, two models have been added inorder to take into account the complete vehicular dynamicsystem in urban condition. Moreover, a novel approach forIMM mode likelihoods updating by using only proprioceptivesensors data has been implemented and tested on severalscenario with degrading GPS signals, particularly on themotorway A86 where tunnels are located. All obtained resultsare encouraging. They introduce complementary works onthis positioning system, namely, a mechanism of denoisingthe received information; and a self calibration to autocorrectfor the system in case of information data loss, such asEGNOS loss.

VI. ACKNOWLEDGMENT

This work has been completed within the POSITIONING,MAPS and LOCAL REFERENCING sub-project (POMA)in the European project CVIS.

REFERENCES

[1] H. A. P. Blom. An efficient filter for abruptly changing systems. In23rd IEEE Conference on Decision and Control, pages 656–658, LasVegas, NV, December 1984.

[2] H. A. P. Blom and Y. Bar-Shalom. The interacting multiple modelalgorithm for systems with markovian switching coefficients. In IEEETransaction on Automatic Control, August 1988.

[3] Ph. Bonnifait, J. Laneurit, C. Fouque, and G. Dherbomez. Multi-hypothesis map-matching using particle filtering. 16th World Congressof ITS Systems and Services, Stockholm, Sweden, 2009.

[4] C. K. Chui & G. Chen. Kalman filtering with real-time applications.Springer-Verlag, second edition, 0:0, 1991.

[5] D. Gruyer, A. Lambert, and B. Mourllion. Etat de lart des stratgiesde fusion de donnes pour la localisation. Rapport du Thme 3, ProjetARCOS, 2004. T3.2.

[6] F. Gustafsson, N. Bergman, and all. Particles filters for positioning,navigation and tracking. IEEE Transaction on Signal Processing.Special issue on Monte Carlo methods for statistical signal processing,2002.

[7] J.E. Handschin. Monte carlos technics for prediction and filtering ofnonlinear stochastique precesses. Automatica, 6:55–563, 1970.

[8] S.J. Julier. The scaled unscented transformation. Automatica, 2000.[9] S.J. Julier and J.K. Uhlmann. A general method for approximating

nonlinear transformation of probability distribution. Technical report,University of Oxford, November 1996.

[10] S.J. Julier and J.K. Uhlmann. A new extension of the kalman filter andnonlinear systems. In Proceedings of the SPIE Aerosense InternationalSymposium on Aerospace/Defense Sensing, Simulation and Controls,Orlando, Florida, September 2001.

[11] R. E. KALMAN. A new approach to linear filtering and predictionsystem. In Transactions of the ASMEJournal of Basic Engineering, 82(Series D): 35-45, 1960.

[12] Francois LeGland. Introduction au filtrage en temps discret. filtre dekalman, filtre particulaire, modles de markov cachs. Master’s thesis,Universit de Rennes 1, 2003.

[13] Franois LeGland. Filtrage particulaire. In Actes du 19me ColloqueGRETSI sur le Traitement du Signal et des Images, volume II, pages1–8, September 2003.

[14] X. R. LI and V. P. JILKOV. Survey of maneuvering target tracking,part v: Multiple model methods. In IEEE Transactions onAerospaceand Electronic Systems, vol. 41, no. 4, 2005.

[15] R. T. MOREO, A. Z. IZQUIERDO, B. U. MINARRO, and A. F. G.SKARMETA. High-integrity imm-ekf-based road vehicle navigationwith low-cost gps/sbas/ins. In IEEE Tans. on Intelligent TransportationSystems, vol. 8, no. 3, pp. 491511, 2007.

[16] B. Mourllion, D. Gruyer, A. Lambert, and S. Glaser. Kalman filterscomparison for vehicle localization data alignment. In ICAR 2005Seattle, Washington, USA, July 18th-20th 2005.

[17] B. Mourllion, D. Gruyer, A. Lambert, and S. Glaser. Kalman filterspredictive steps comparison for vehicle localization. In IEEE/RSJInternational Conference on Intelligent Robots and Systems (IROS05),Alberta, Canada,, August 2-6, 2005 2005.

[18] A. Ndjeng Ndjeng, S. Glaser, and D. Gruyer. A multiple modellocalization system for outdoor vehicles. In Proc. IEEE InternationalSymposium, Istanbul, 2007 2007.

[19] A. Ndjeng Ndjeng, D. Gruyer, and S. Glaser. New likelihood updatingfor the imm approach application to outdoor vehicles localization. InIROS09, Saint Louis, USA, Octobre 2009.

[20] M. Norgaard, N. K. Poulsen, and O. Ravn. Advances in derivative-free state estimation for nonlinear systems. In Revised edition of thetechnical report IMM-REP-1998-15, Technical University of Denmark,2000.

268