An overview of positioning and data fusion techniques applied to land

vehicle navigation systems. Denis Gingras, Dr. Ing. Department of Electrical and Computer Engineering, Université de Sherbrooke, Sherbrooke (Québec) Canada, J1K 2R1 Canadian NCE AUTO21 Research Theme Coordinator "Intelligent Systems and Sensors" Email: denis.gingras@usherbrooke.ca ABSTRACT

In this paper, we will review the problem of estimating in real-time the position of a vehicle for use in land navigation systems. After describing the application context and giving a definition of the problem, we will look at the mathematical framework and technologies involved to design positioning systems. We will compare the performance of some of the most popular data fusion approaches and provide some insights on their limitations and capabilities. We will then look at the case of robustness of the positioning system when one or some of the sensors are faulty. We will describe how the positioning system can be made more robust and adaptive in order to take into account the occurrence of faulty or degraded sensors. Finally, we will go one step further and explore possible architectures for collaborative positioning systems, whereas many vehicles are interacting and exchanging data to improve their own position estimate. We close the chapter with some concluding remarks on the future evolution of the field. Keywords: positioning, localization, navigation, Kalman filter, particle filter, neural network, , geo-referencing, road transportation, vehicle, car, GIS, ADAS, intelligent system, Bayesian estimation, sensor fusion, data processing, GPS receiver, odometer, inertial, gyroscope, IMU, INS, tracking, ITS, ADAS, autonomous driving assistance systems. INTRODUCTION The field of automotive positioning systems has become a research topic in full rise these last few years. Generally speaking, positioning information in road transportation is of prime importance for safety reasons. We need to know where we are (vehicle position) and we also need to locate obstacles and other objects/vehicles in the vicinity and nearby environment of our own vehicle. So an approach able to precisely localize our vehicle and evaluate its environment on the road is required to increase safety (Drane & Rizos, 1997). Apart from safety issues, the next generation of vehicles will allow the driver and passengers to have access to a broad range of services, such as path planning, navigation, guidance and tracking, which are based on information

technologies, telecommunications and telematics. Future vehicles are likely to be as well mobile offices, information centers on wheel, or e-nodes connected to the web and other networks (Kim, Lee, Choi & al., 2003). To supply those services and to provide the adequate information contents to the driver and passengers, it is required to determine in real-time the vehicle’s position as accurately and efficiently as possible. This function is an essential part of an integrated navigation information system (INIS). An INIS embedded in a vehicle is basically composed of a geographic information system (GIS), a database composed of roadmaps, cartographic and geo-referenced data, a positioning module, a human-machine interface (HMI) as well as computing and telecommunication capabilities. It provides useful functionalities to the driver like path planning, guidance, digital map and points of interest directory (Farrell & Barth, 1999). The guidance module uses a planned trip to indicate the driver which route to take. To avoid giving wrong indications and impair driving safety, the navigation system relies upon a positioning module to know precisely and continuously the localization of the vehicle. In this chapter, we will focus on the positioning module of INISs.

Nowadays, the most popular sensor used for positioning is a global positioning system (GPS), also called a GPS receiver. However, GPS alone based systems suffer from various problems such as poor data latency, signal multipath errors and occlusions of satellite signals. To alleviate these problems and obtain the required performance of the positioning module, we usually use a combination of heterogeneous and complementary sensors whose measurements are integrated or fused in real time (Cannon, Nayak & al., 2001, Grewal, Weill & Andrews, 2001). Among the other types of sensors, we have Distance Measurement Indicators (DMI), such as differential odometers, and 2 or 3 axis inertial measurement units (IMU). Two or more of these complementary positioning sensing methods must be integrated together to achieve the required performance at low cost. The data integration, which implies the fusion of noisy signals provided by each sensor, must be performed in some optimal manner. Basically the goal of data fusion in positioning systems is 1) to fill in the time gaps whenever we face a loss of position estimate and 2) to improve the position estimate by ensemble averaging and exploiting information redundancy. Historically, data fusion for positioning has been applied in robotics (Roumeliotis & Bekey, 2000, Durant-Whyte, 1994) as well as in military, space and avionic applications (Suresh & Sivan, 2004). Multi-sensor data fusion system design based on rapid prototyping software platform and Kalman filtering for land navigation has also been proposed (Abuhadrous, Nashashibi, Laurgeau & Chinchole, 2004, Redmill, Martin & Özgüner, 2000). Hence, with the advent of more powerful, accessible and cheaper technologies embedded in vehicles for road transportation, on-board navigation systems are becoming feasible and standard in the automotive industry.

The chapter is constructed as follow. After the present introduction, we will look at the specific problem of vehicular positioning and localization, what are the systems and sensors available to do it and what are the problems to be solved. In the following section, we will consider the most common data fusion architectures when multiple positioning sensors are used. Positioning system designers usually choose the Kalman filter or a variation thereof as the data fusion method. We will then look at the various implementations possible, such as the extended Kalman filter (EKF), the unscented Kalman filter (UKF) (St-Pierre & Gingras, 2005) and particle filters (PFs) (Arulampalam, Maskell & al., 2002), which are generalizations or variations of the Kalman filter used to cope with nonlinearities of the sensors or with non-gaussianity of the signals. To complete the survey, we will also present an interesting alternative to classical Kalman filtering based on the use of artificial neural networks (NNs) (St-Pierre & Gingras, 2004).

After having surveyed those various data fusion approaches and their respective performances, we will then focus our discussion on the reliability and robustness requirements for a real time positioning system. In positioning navigation systems, at any time, any of the sensors can break down or stop sending information properly, temporarily or permanently. To ensure a practical solution for use in guidance and navigation systems, faulty sensors must be detected and isolated such that their erroneous data will not corrupt the global position estimates. As we just mentioned, Kalman filter is usually popular for data fusion applications. However, an interesting idea is to use it for fault detection architecture as well. We will then evaluate the potential of combining fault detection and data fusion into a single architecture to make a robust positioning navigation system (Guopei & Gingras, 2007).

In the subsequent section, we will then consider the possibility to achieve a distributed collaborative architecture. Proliferation of real time inter-vehicular communications provides new sources of exploitable positioning data. Vehicles can, under numerous situations, have GPS satellite shortages but there will always be vehicles in their vicinity, viewing different GPS satellites line of sights (LOS), to provide them with useful navigation information. We will thus look at a cooperative positioning technique making use of reliable positions of some vehicles to enhance positioning estimates of some others (Abdessamie & Gingras, 2008). The approach, based on a simple idea, exploits inter-vehicle data flow to extract good position measurements from vehicles with good GPS satellite LOS, in order to enhance low positioning accuracy of other vehicles, in the neighborhood. The integration of such information is done using geometric data fusion approach.

Finally, we conclude the chapter with a discussion on future trends in the field, what are the challenges ahead and what can be expected for future positioning systems. Some general comments are provided along with proper references. CONTEXT AND PROBLEM STATEMENT Modern automotive navigation applications require position estimates with a precision of 1 meters or less and at a frequency of 1 Hertz or more. Basically three types of positioning architecture can be used to locate a vehicle in its environment (Laneurit, Blanc & al., 2003)

The first type involves absolute localization, that is, a global position of the vehicle is given in the world reference by some exteroceptive sensors such as a GPS receiver. The performance obtained often depends on satellites visibility and actual satellites configuration at the time of measurement. Good satellites configuration are one which present a sufficient elevation angle, a good signal to noise ratio and a good geometrical configuration of the satellites. A differential GPS receiver alone could in principle estimate the position with the required performance. However, it is still much too expensive for automotive applications and the fixed references are not always available. On the other hand, a low cost GPS receiver cannot always achieve the required precision and reliability as it suffers multipath signal propagation and frequent signal outages, most specifically in urban areas.

The second type involves relative localization, that is, when the position and orientation of the vehicle are determined from its previous position. Integrative sensors, like odometers and inertial systems, are commonly used in relative positioning. This type of measurement, however, involves time derivation. We therefore observe a rapid divergence of the localization when distance increases due to cumulative errors in the estimate. This is the main drawback of this family of sensors. Two popular sensors used are the differential odometer and the inertial measurement unit (IMU) (Abbott & Powell, 1999). A differential odometer measures the distance traveled by a

vehicle and the current azimuth during a sampling period. An inertial measurement unit (IMU) measures the vehicle's inertia characterized by its acceleration and its angular velocity. An IMU measures the acceleration and the angular velocity along the axis of a Cartesian coordinate system. With these two sensors, the position of the vehicle is reckoned by applying basic kinematics’ equations and using an initial position obtained from another information source. By means of integration, the traveled distance and the azimuth variation can be computed and therefore a new position can be obtained with these measurements and the last known position by dead reckoning. The estimated position will eventually drift from the real position because of the accumulation of errors. Indeed, the recursive nature of the positioning computation with the IMU causes the positioning error to grow proportionally with time. Typically, DMIs (odometer/IMU) can cumulate a drift in the position estimate of the order of 10m to 15m after ten minutes of integration. A periodic reset of those relative localization systems is therefore needed.

The third type of positioning architecture consists of a hybrid form of localization, that is, this method combines the two previous ones in order to cumulate their advantages and to alleviate their drawbacks. The third type, which we will consider from now on, implies data fusion from the various sensors. A trivial way to fuse the data from these two sensors is therefore to reset periodically the IMU position estimate with an absolute position estimate from the GPS. A more complex fusion method than reckon/reset positioning system described above is required to improve the precision of the estimation. These methods fuse continuously the available measurements in some optimal sense, as depicted in figure 1 in a centralized architecture.

Figure 1 : Centralized data fusion architecture

One important issue is data registration and latency correction. Odometer and INS signals are received with a nearly constant latency (this error is usually far less than lms). However, GPS positioning calculation time and reception delay may vary largely. Fortunately, it is possible to calculate those delays using the Pulse Per Second GPS signal and introduce them in the data fusion algorithms. When the latency of the GPS data is low, the position information can be propagated forward using the velocity data and a constant vehicle velocity model. This is usually adequate for slow moving vehicles. Problems occur, however, when the latency of the GPS data is high and the vehicle’s dynamic is fast. For example, if the GPS latency is 0.2s and the vehicle is travelling at 30 m/s (110 km/hr), then the vehicle will move six meters ahead while storing the IMU data. If the vehicle is moving at constant speed following a straight path, then the position estimate can simply be adjusted forward accordingly. However, any curve or any deviation from a straight line will lead to incorrect position estimates. Simple straight GPS correction is not sufficient in that case and a more sophisticated latency correction algorithm is required before applying and fusing the GPS data with the current inertial state.

Fusion Method

Estimated Position

GPS

IMU

Odometer

Inclinometer

The sensors’ measurements are also distorted by deterministic and random errors. Those sources of random errors are usually described using stochastic models in a statistical framework. The estimate ideally maximizes the a posteriori probability of the random variable position, resulting from the mathematical transformation of the stochastic processes, which model the sensors’ imperfect measurements. The fusion method then defines the mathematical transformation. The architecture of data fusion system for positioning can be decentralized or centralized. In the centralized architecture, all the sensor measurements are fused by one fusion method only. So it is easy to compare the performance of two different fusion methods when the cluster of sensors is the same. Before looking into the details of data fusion architectures, we will now look at the different sensors available for localization. SENSORS MODELS In a positioning navigation system, various sensors are used. Generally speaking, a sensor is a device that responds to or detects a physical quantity and transmits the resulting signal to a controller. Positioning sensors can be designed to detect various parameters (coordinate, distance, direction, or angular velocity) of the position of vehicular mechanical systems. Sensors can provide absolute or relative navigation information. GPS and magnetic compass provide absolute position and angular direction (azimuth) respectively, whereas all other sensors provide relative navigation information using dead reckoning. Detailed relationships between the position and typical sensors are shown in Table 1.

Sensor Output Relation to position

GPS Rover position

Directly output position coordinates

IMU Accelerations and angular rate

Outputs can be integrated by an INS to obtain the vehicle position

Odometer

Distance or increment of distance

Inclinometer Inclination

Magnetic compass Azimuth

Position coordinates are determined by dead reckoning from the distance and direction relative to a known location

Table 1. Relationship of Vehicle Position and Sensor Outputs

GPS model

The GPS model can be described by equation (1) (Farrell & Barth, 1999).

ˆ[ ] [ ] [ ] [ ] [ ] [ ]trop iono white multt t t t t tρ ρ δ δ δ δ= + + + + (1)

where ˆ[ ]tρ is the pseudo-range measured by the emulated receiver, [ ]tρ is the real pseudo-range, [ ]trop tδ is the tropospheric delay, [ ]iono tδ is the ionospheric delay, [ ]white tδ is the white

noise generated by the receiver's electronic components and [ ]mult tδ is the multipath problem.

IMU model The 3-axis inertial measurement unit has a single model for every gyroscope and accelerometer described by equation (2), (Grewal, Weill & Andrews, 2001).

1

2

ˆ [ ] [ ] ( [ ] [ ]*sin( )

[ ]*sin( )) [0] [ ]

[ ]*sin( ) [ ]*sin( )

( [ ] [ ]*sin( ) [ ]*sin( ))

[ ],

i i i i j ij

t

k ik i bin

j ij k ik

i i j ij k ik

i

d t d t s d t d t

d t b w n

d t d t

c d t d t d t

w t

δ

δ

δ δ

δ δ

=

= + +

+ + +

+ +

+ + +

+

∑ (2)

where ˆ [ ]id t is the measurement, [ ]id t is the real value, is is the scale factor error, [ ]jd t is the real

value relative to the axis j , [ ]kd t is the real value relative to the axis k, ijδ is the misalignment

angle between the axis i and j, ikδ is the misalignment angle between the axis i and k, [0]ib is the turn-on bias, [ ]biw n is the random walk white noise characterizing the bias drift, ic is the non-linear scale factor and [ ]iw t is the additive white noise component.

The scale factor error, the bias parameters, the non-linear scale factor and the additive white noise component have different values depending on the sensor type. Differential odometer model DMIs consist of wheel rotation sensors, such as differential odometers and ABS (anti-breaking systems) sensors (Bonnifait, Bouron, Meizel, & Crubille, 2003). Those are the one usually used in automotive localization systems. The wheel revolutions are integrated from them to measure the total distance traveled by the vehicle. Given time information traveled, a forward velocity is determined. As opposed to GPS receivers, DMIs are not subject to signal masking or outages. However the positioning errors cumulate with time, as is the case for the IMUs. A differential odometer is constituted of two sensors measuring the number of rotation of each wheel situated on the same axle. The total traveled distance and the azimuth of the vehicle can be computed with the equations (3) and (4) respectively.

[ ]

ˆ ˆ[ ] [ 1](1 * [ ] )* [ ]

2(1 * [ ] )* [ ] [ ]

2

d d d

g g g

d t d tv v t s c t r t

v v t s c t r t

= − +

+ + ++

+ + +

(3)

[ ]

ˆ ˆ[ ] [ 1](1 * [ ] )* [ ]

(1 * [ ] )* [ ] [ ] ,

g g g

d d d

t tv v t s c t r t

lv v t s c t r t

l

θ θ= − +

+ + +−

+ + +

(4)

where ˆ[ ]d t is the total traveled distance at time t, ˆ[ 1]d t − is the total traveled distance at time t-1, [ ]v t is the car's velocity, dv and gv are the gains characterizing the tire dilatation, ds and gs are

the scale factors for the right and the left wheel respectively, [ ]dc t and [ ]gc t are the numbers of

rotations measured within the interval [t-1,t] for the right and left wheel respectively, [ ]r t is a

uniform random variable describing the resolution error, ˆ[ ]tθ is the azimuth estimated at time t, ˆ[ 1]tθ − is the azimuth estimated at time t-1 and l is the axle length.

DATA FUSION TECHNIQUES The Kalman filter The most basic data fusion method for localization is based on the Kalman filter (Kalman, 1960). It is a set of mathematical equations that provides an efficient computational means to estimate the state of a process, in a way that minimizes the mean square error. This filter is very powerful in several aspects: it supports estimations of past, present, and even future states, and it can do so even when the precise nature of the modeled system is unknown. The Kalman filter is an optimal linear estimator, which uses the a priori information on the sensor noise sources, the vehicle dynamic and the kinematics equations to compute recursively an optimal position, while minimizing the mean square error (Gelb, 1974). The filter is optimal when the process noise and the measurement noise can be modeled by white Gaussian process. The recursive implementation of Kalman filters is well suited to the fusion of data from different sources at different times in a statistically optimal manner.

Many other filter designs can be shown to be equivalent to the Kalman filter, given several constraints. The recursive sequence involves prediction and update steps. The prediction step used a dynamics model that describes the relationship between variables over time. A statistical model of this dynamic process is also necessary. A prediction is usually done to estimate the variables at the time of each measurement, as well as in between measurements when an estimate is required. The measurement update combines the historical data passed through the dynamics model with the new information in an optimal fashion. The Kalman filter addresses the general

problem of trying to estimate the state nx ℜ∈ of a discrete-time controlled process that is governed by the linear stochastic difference equation We then have,

1 1 [ ] kk k k k k kx x w+ += Φ + (5)

1 11 1[ ]k kk k k kz H x v+ ++ += + , (6)

where 1k kx + is the predicted process state vector, k kx is the estimated process state vector, 1k k+Φ is

the discrete state transition matrix from k to k+1, kw is the process noise vector, 1kz + is the measurement vector, 1k kH + is the observation matrix and 1kv + is the measurement noise vector.

Supplied with initial conditions PO and xO, the prediction equations can be given by

kkk xx ˆˆ 1 φ=−+ (7)

kTkkkk QPP +=−

+ φφ1 (8) and the update equations are given by

( ) 1−−− += kTkkk

Tkkk RHPHHPK (9)

( )−− −+= kkkkk xHzKxx ˆˆˆ (10)

( ) −−= kkkk PHKIP , (11) Where x̂ is the vector of estimated states, P is the covariance of estimated states, Q is the dynamics noise matrix, K is the Kalman gain matrix, R is the covariance matrix and z is the vector of observations.

Although an error state Kalman filter is desirable in some applications, a state space model will be preferred here as it most clearly illustrates the operation of the filter. Rigorous development of a Kalman filter requires a great deal of work in understanding the physics and electronics involved in each sensor to understand what the error sources will be. A comprehensive model of all of the necessary variables to match as closely as possible the real world phenomenon is needed. Given the specifications of the project, a sensitivity analysis would then be done to decide which variables may be ignored or lumped together. This often involves Monte Carlo simulations with several likely candidate filters and repeated tuning of the statistics. The filter must be able to operate with the allowed throughput and processing restrictions. Finally, blunder detection, adaptive filter gains, and practical limits to covariance must be set to achieve optimum performance.

The Centralized Kalman Filter (CKF) is the most common filter design implemented in integrated navigation systems. However, it behaves poorly in the presence of nonlinearities. In the following sections, we will look into more details of the use of the various improvements possible over the basic Kalman filter for an integrated navigation information system. The extended Kalman filter

Improvement of the basic Kalman filter architecture can be achieved in nonlinear situations with the extended Kalman filter (EKF) (Anderson B. & Moore J.,1979, Brown R. G. & Hwang P. C., 1997). This filter is based upon the principle of linearizing the state transition matrix and the observation matrix with Taylor series expansions. The extended Kalman filter has been very popular for land navigation system (Abbott & Powell, 1999, Wu, Gao & Wan, 2002, Redmill, Kitajima & Özgüner, 2001). The equations of a centralized data fusion architecture based on an extended Kalman filter for land navigation positioning system are described in (Abbott & Powell, 1999). As with the Kalman filter, the extended Kalman filter predicts the states of the random process using equation (5). The predicted states are updated with the measurements in equation (6). In our study presented in (St-Pierre & Gingras, 2005), we have 13 states to describe the random process. A position-velocity-acceleration model is used for each component of the position (Brown & Hwang, 1997). The last four states include the slope, the pitch, the azimuth and the yaw velocities. The state transition matrix 1k k+Φ is linear. Only the observation

matrix 1k kH + contains nonlinear equations, the most relevant for horizontal positioning is

described by equation (12).

cos( )*cos( ) sin( ) cos( )*sin( )sin( )*cos( ) cos( ) sin( )*sin( ) *

sin( ) 0 cos( )

R y p y y p N

P y p y y p E

Y p p D

a aa aa a

⎡ ⎤Φ Φ − Φ Φ Φ⎡ ⎤ ⎡ ⎤⎢ ⎥⎢ ⎥ ⎢ ⎥= Φ Φ Φ Φ Φ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥− Φ Φ⎣ ⎦ ⎣ ⎦⎣ ⎦

(12)

where Ra , Pa , Ya are the acceleration vector components along the roll, the pitch and the yaw axis respectively, pΦ and yΦ are the Euler angles for the pitch and the yaw axis respectively, Na , Ea , Da are the acceleration along the north axis, the east axis and the down axis respectively. The extended Kalman filter approximates the nonlinear matrix H based on the Taylor series expanded about the estimated state vector with

1 1

ˆ[ ]ˆ ˆ ˆ ˆ[ ] [ ] ( )

ˆk k

k k k k k k k kk k

H xH x H x x x

x+ +

∂≈ + −

∂. (13)

The linear approximation often introduces large errors in the estimated state vector and can lead to the divergence of the filter. As expected, the linearization can lead to poor performance and divergence of the filter for highly non-linear problems. In addition, the performance analysis of the extended Kalman filter presents some difficulties due to the recurrence of the measure sequence into the states of the filter (Grewal, Weill & Andrews, 2001). Finally, implementation of the extended filter can be quite laborious depending on the number of states required to model the system. For all these reasons, a recent improvement to the EKF, named the “unscented” Kalman filter (UKF) has been proposed (Julier & Uhlmann, 1997).

The Unscented Kalman filter The UKF approximates the posterior probability density resulting from the nonlinear transformation of a random variable instead of approximating the nonlinear functions with a Taylor series expansion. The approximation is done by evaluating the nonlinear function with a

minimal set of carefully chosen sample points using a deterministic sampling approach. The posterior mean and covariance estimated from the sample points are accurate to the second order for any nonlinearity (Merwe, Freitas & Wan, 2000). If the priori random variable is Gaussian, the posterior mean and covariance are accurate to the third order for any nonlinearity (Wan & Merwe, 2000). The first use of an unscented Kalman filter for land navigation positioning system is described in (Julier, 1997). Apart from our work (St-Pierre & Gingras, 2005), one paper has been recently written on the use of the unscented Kalman filter as the fusion method in an integrated navigation information system (Li & Leung, 2003). An unscented Kalman filter has also been used for GPS positioning (Mao, Wada & al., 2003).

The unscented Kalman filter is based on the unscented transformation, which is a method for reckoning the statistics of a random variable undergoing a nonlinear transformation. A set of 2*nx+ 1 weighted samples are deterministically chosen to capture the true mean and variance of the prior random variable.

x w vn n n nχ = + + (14) where xn is the number of process states, wn is the dimension of kw and vn is the dimension of

kv . The unscented Kalman filter approximates the nonlinear observation matrix by

2*

, 11 , 10

ˆ[ ] * [ ]n

x vi i kk k i k k

iH x W H

χ

χ χ ++ +=

≈ +∑ (15)

Where, Wi are the weights, , 1

xi k kχ + are the sigma points describing the prior predicted states

and , 1vi kχ + are the sigma points describing the measurement noise.

In order to obtain statistically reliable data on the performance of both algorithms, one hundred Monte Carlo simulations have been run for each sensor fusion method (St-Pierre & Gingras, 2005). For each sampling time, the estimated positions from the Monte Carlo simulations form the sampling distribution. There were 26639 measurement vectors for each Monte Carlo simulation. These sampling distributions approximate the truth continuous distributions of the posteriori random variables describing the estimated positions. The first moment of each sampling distribution has been computed and used for the computation of the performance metrics. The two performance metrics usually encountered in data fusion systems analysis are the accuracy/precision of the fusion and the computational time to perform the fusion. The accuracy is evaluated by taking the Euclidian distance between the estimated position and the true position. The mean and the variance of the Euclidian distances for the whole simulation are reckoned. The variance describes the precision of the fusion method. The horizontal position is described by the tangential plane located at the real vehicle position whose coordinates are given by the latitude and the longitude.

In (St-Pierre & Gingras, 2005) we gave detailed results showing that the unscented Kalman filter has slightly better results for horizontal positioning than the extended Kalman filter. The estimated position is less biased for the unscented Kalman filter than for the extended Kalman filter. It shows also that the unscented Kalman filter is more precise than the extended Kalman filter. However, contrary to the claim in (Merwe, Freitas & Wan, 2000), (Wan & Merwe, 2000),

we found that the computational cost of the unscented Kalman filter is significantly greater, by a factor larger than 20, than the one of the extended Kalman filter. The significant execution time difference is related to the number of times equations (5) and (6) are evaluated for each fusion algorithm. With the unscented Kalman filter, these equations are evaluated 75 times, once for each sigma point. With the extended Kalman filter, the Taylor series expansion of these equations is evaluated only once at each iteration. Furthermore, the Jacobian of the matrix H used in the Taylor series expansion is calculated only once because the observation equations are static. Thus the multiple computations of equations (5) and (6) by the unscented Kalman filter at each iteration is responsible for the larger computational cost.

Surprisingly, we found that the unscented Kalman filter is less performing than the extended Kalman filter when there is no GPS solution available (St-Pierre & Gingras, 2005). In that situation, the acceleration of the vehicle measured by the IMU is used to estimate the vehicle’s position described by equation (12). This equation represents the nonlinear transformation of the estimated states which are assumed to be Gaussian random variable in order to predict the IMU measurement. The performance of both filters depends on their capacity to estimate the mean of the resulting random variable. An empirical analysis has been made to evaluate this capacity. In this experiment, each state has been modeled by a discrete Gaussian random variable with 100 realizations distributed uniformly in the range of possible values with a 99% probability of realization. Each realization is present a number of times proportional to its probability of realization in the statistical data representing the probability function. Thus, 24060 samples modeled each random variable. The nonlinear function described by equation (12) is then applied to these random variables and the means of the resulting random variables are computed. The same discrete random variables have been used with the Taylor series expansion of equations (12) and (13). In the extended Kalman filter, the linearization occurs around the states estimated at the previous iteration. The linearized equation is applied to the predicted states at the current time. The linearization error is directly proportional to the difference between the estimated states and the predicted states. For the empirical analysis, the mean variation between the estimated value and the predicted value obtained with the extended Kalman filter for one Monte Carlo simulation has been taken. Table 2 shows the percentage variation between the real mean of the a posteriori probability density and the estimated mean of the a posteriori probability density obtained with the Taylor series expansion and the unscented Kalman filter respectively. As can be seen, the unscented Kalman filter provides no significant improvement over the extended Kalman filter and even brings some degradation in performance for two acceleration components (roll and pitch).

Estimated state EKF UKF Roll acceleration 0.0064 % 0.8070 % Pitch acceleration 0.0218 % 1.2876 %Yaw acceleration 0.2482 % 0.0754 %

Table 2: Difference between the real mean and the estimated mean of the a posteriori density The superiority of the unscented Kalman filter happens only when the variation between the

predicted states and the estimated states is important. However, due to the low dynamics of the vehicle, most of this variation is not important enough to generate a significant linearization error for the EKF.

The particle filter

More recently, a new class of sequential stochastic methods (Monte Carlo) for Bayesian filtering, called particle filters (PF), has been proposed to solve the multi-sensor fusion problem in land navigation, tracking and positioning systems (Caron F, Davy M. & al. in press, Arulampalam M. S., Maskell S. & al., 2002). Generally speaking, Kalman filters are special cases of Bayesian filters suitable to estimate posterior probability distributions when linearity or gaussianity are assumed. However, this is not always a valid hypothesis. The usual standard approach consists of making model simplifications or crude analytic approximations to obtain algorithms that can be easily implemented. However these simplifications bring undesired behaviour of the systems as we have seen with the Kalman filter in real situation. With the recent availability of high-powered computers, numerical-simulation based approaches can now be considered and the full complexity of real problems can be addressed. Although these integration and/or optimization problems can be tackled using analytic approximation techniques (ex. EKF) or deterministic numerical integration/optimization methods (ex. UKF) these classical methods are often not sufficiently precise or robust, or they are simply too complex to implement. In these situations, Monte Carlo algorithms become an attractive alternative. These algorithms are remarkably flexible and extremely powerful (Doucet A. & Wang X. 2005). The basic idea is to draw a large number of samples (named particles here) distributed according to some probability distributions of interest so as to obtain consistent simulation based estimates.

As opposed to the Kalman filter based algorithms, those particle filters are more general and do not assume Gaussian posterior probability distributions (Andrieu & Doucet 2002). In general, those distributions have a very complex shape, and cannot be calculated in closed-form as it requires integration and optimization of complex multidimensional functions. They can be arbitrary multimodal or highly skewed posterior probability distributions. This kind of distributions may arise in positioning systems when faced with nonlinear observations, evolution models or non-Gaussian noise sources. This occur for instance in the presence of sensor failures or abrupt changes in sensors’ functioning conditions. Particle filters or sequential Monte Carlo methods for Bayesian filtering provide a numerical approximation of those posterior and marginal posterior probability distributions using a set of weighted random samples, called particles.

Particle filters can deal with hybrid (both continuous and discrete). state vectors. In most cases, the trade-off between estimation accuracy and computational burden comes down to adjusting the number of particles used. The standard particle filter algorithm is based on sequential importance sampling, with weight normalization. A detailed mathematical description, of the standard particle filter algorithm can be found in (Caron, Davy. & al., 2007) and (Doucet & Wang, 2005). Results and performances reported in the literature (see references above) so far show that significant improvements can be achieved in general dynamic systems, where non Gaussian posterior distribution and nonlinearities have to be dealt with (Crisan & Doucet, 2002).

Data fusion using Neural networks A centralized fusion usually implied the use of only one algorithm, which fuses all the measurements provided by the sensors. The engineering labour required to construct a centralized Kalman or particle filter can be very high. For this reason, most of the GPS/INS systems have decentralized filters, which gain in simplicity. However, the price to pay is a loss of precision compared with a centralized architecture (Knight, 1997). An interesting alternative is to use a neural network (NN) for the data fusion. Indeed, a centralized NN is no more difficult to realize than a decentralized one. NNs have already been applied to data fusion related to positioning problem in robotic with success (Kobayashi & Arai, 1998, Beattie, 1998, Hagan and Menhaj,

1994). The major advantage of an NN compared to a standard Kalman filter resides in the fact that it does not need any a priori statistical and mathematical model to find a function, which maps optimally the inputs with the outputs, in our case the absolute position of the vehicle. The most difficult task however, is to gather sufficient training sensor data, which can cover adequately the different manoeuvres to be encountered by a road vehicle and its dynamic. The manoeuvres are a function of the road geometry and the vehicle's performance. For example, a NN trained with measured data coming only from a straight road segment may not be able to give a good position when the vehicle meets a curve.

As an example, we will consider the architecture described in more details in (St-Pierre & Gingras, 2004). We used a feed-forward back-propagation neural network that was trained with 14939 training data set for 2000 epochs. The NN has 4 layers composed of 20, 20, 20 and 3 neurons respectively. The corresponding transfer functions are linear, log-sigmoid, tan-sigmoid and linear. This architecture was the most promising of the 12 architectures investigated with various numbers of layers, transfer functions and number of neurons trained initially for 100 epochs. Batch training was preferred over iterative training for its computation efficiency. The mean square error gave the training performance at each epoch. The second order training method was the scaled conjugate gradient. This method requires only O(N) operations per epoch compared to others methods like Gauss-Newton O(N3) and Levenberg-Marquardt O(N3) (LeCunn & Bottou, 2001, Hagan & Menhaj, 1994). When considering the network size, the other training methods were impractical.

Data Sensor

Latitude GPS

Longitude GPS

Altitude GPS

Percent Dilution Of Precision (PDOP)

GPS

Linear acceleration INS

Horizontal centripetal acceleration

INS

Vertical centripetal acceleration

INS

Yaw angular velocity INS

Pitch angular velocity INS

Total traveled distance Differential odometer

Azimuth Differential odometer

GPS solution availability GPS

Sampling time IMU

Table 3 contains the NN's inputs.

The GPS receiver sampling frequency is usually lower than those of the inertial measurement unit or the differential odometer. For synchronization purpose, a Boolean entry specifies to the NN if a GPS solution is available or not. A centralized Kalman filter has also been realized as a reference for the comparative evaluation of the NNs performances. The centralized Kalman filter has 13 states and 10 measurements. The simulation generated 26430 data samples.

The mean and variance of the positioning error during the simulation was computed. Table 4 indicates that the positions estimated by the NN are less biased for the longitude and altitude but more biased for the latitude than the same positions estimated by the Kalman filter. The NNs estimation biases do not exceed 10 meters. So even if the NN is a biased estimator, it still meets the required performance.

As shown in Table 5, the variances of the latitude errors and the altitude errors for the NN are less than those of the Kalman filter. The variance of the longitude errors is 5 percent more for the NN than for the Kalman filter. In our particular case, the performance of the NN is generally better than the performance of the Kalman filter in a mean square error sense. The important improvement for the altitude is caused by the NNs ability to estimate the bias of the GPS altitude.

Position Kalman (m) ANN (m) Improvement (%)

Latitude 2.73 9.34 -242.90 Longitude 12.44 3.56 71.38 Altitude 49.21 0.38 100.78

Table 4: Mean of the positioning errors by the Kalman Filter and the NN

Position Kalman ANN Improvement (%)

Latitude 1073.2 606.1 43.52 Longitude 994.8 1029.7 -3.51 Altitude 12952.0 2.4 99.98

Table 5: Variance of the positioning errors by the Kalman filter and the NN It can be seen here that NNs can be used as a centralized fusion method. The results show that

neural networks may be an attractive alternative to the Kalman filter as a centralized fusion method. In (St-Pierre & Gingras, 2004), we showed also how NNs can also be used prior to fusion as nonlinear pre-processing filters for the land navigation positioning problem. Similar use of NNs was applied successfully to telecommunications problems (Chuah & Sharif, 2001). In our paper, we showed the NNs capability to successfully learn nonlinear functions when applied to GPS and differential odometer measurements. The major difficulty with an NN is to have access to sufficient ground truth data for the supervised training. Usually, the only solution is to have

access to some data coming from a reference, usually given by high precision and high cost sensors. Some further research on this topic would include the evaluation of various NNs with real sensors and the replacement of the feed-forward back-propagation NN with other types of NN such as recurrent NNs. More exhaustive comparisons with other data fusion techniques are also required.

ROBUST AND ADAPTIVE POSITION ESTIMATE IN PRESENCE OF SENSORS FAULTS So far, we considered mainly the case of continuously operating noisy sensors. But what happen to the data fusion process and the position estimate if one of the sensors fails? Either from a user’s safety point of view or a designer’s perspective, all automotive navigation systems should be fully reliable and prevent faults or failures. In all but the most trivial cases the existence of a fault may lead to situations with safety, health, environmental, financial or legal implications. Although good design practice tries to minimize the occurrence of faults and failures, it is recognized that such events do occur. In such cases, faulty sensors must be detected and the system must be able to reconfigure itself so as to overcome the deficiency caused by the fault. In brief, a navigation system must be robust and adaptive.

Faults can cause the loss of the overall performance of a system, which may present hazards to personnel or lead to unacceptable economic loss. In order to minimize the impact, fault detection schemes must be developed. In addition to the potential use of particle filters described in the previous section, several other fruitful research efforts in the field of fault detection and filter based adaptive architectures, combining fault detection and data fusion, have been proposed to improve the reliability and adaptability of various control systems (Patton, Frank & Clark, 1989, Pouliezos & Stavrakakis, 1994, Isermann, 2005). However, little has been published so far in the area of automotive navigation systems.

Fault detection architecture A fault is usually defined as an undesired change in system estimated parameters that degrade partial or overall performance. Fault detection is a binary decision making process. Either the system is functioning properly, or there is a fault present. Generally speaking, fault detection consists of two processes: residual generation and decision making, as shown in Figure 2 (Hashimoto, Kawashima & al. 2001, Magrabi & Gibbens, 2000). Residual Generation Residuals are defined as the resulting differences between analytically redundant quantities in the system model. These are similar to innovations generated by a Kalman filter, which are the differences between the measured and estimated outputs. Under normal conditions, residuals are small or zero mean; while the occurrence of a fault causes the residuals to go to non-zero or unusually large values. Decision Making The decision making process, which acts as an arbitrator, involves assessing the residuals and identifying when and where any abnormalities occur. This is done through threshold testing both static and dynamic residual behaviors, and various statistical tests, where the thresholds are typically based on signal/residual variance.

Figure 2: Fault Detection Architecture Sensor faulty models So far, although the precision and reliability of sensors are improved significantly with the development of the technology, various sensor faults driven by different situations do exist. In the following, several faulty scenarios of sensors are investigated and discussed. Global positioning system (GPS) faulty model A low-cost GPS receiver can output the vehicle position and driving speed. However, the measurement is likely to be corrupted by time-correlated noise and the GPS signal is susceptible to jamming. However, the position and velocity measurements do not drift over long periods of time. A GPS faulty model can be based on four particular parts: typical error budget, environmental interferences, signal loss, and hardware malfunction.

Typical Error Budget The main error sources in GPS are listed in Table 6. These errors can be divided into two categories (Farrell &Barth, 1999, Kronander, 2004): common and non-common. Common errors are approximately the same for receivers operating within a limited geographic region. Non-common errors are unique to each receiver and depend on the receiver type and multipath mitigation technique being used (if any). The point of this classification is that DGPS can effectively remove the common errors.

Source of errors Standard deviation (m) Common Ionosphere Clock and ephemeris Troposphere Non-common Receiver noise Multipath

7.0 3.6 0.7 0.1~0.7 0.1~5.0

Table 6 GPS Error Sources and their Approximate Deviation [27]

Environmental Interferences

fault

no fault

Sen

sor O

utpu

ts

Residual Generation

Decision Making

GPS satellite signals, as with any other radio signals, are subject to some forms of interference and jamming. It is known that GPS satellite currently transmit position information in the 1,500-MHz frequency band with a typical accuracy under 100 meters to anyone in the world who has a simple receiver costing as little as $100. Any electronic systems generating radio signals in this frequency band, main lobe or side lobe, will tend to be a source of inference to the GPS receiver. With the popularization of personal radio and Wi-Fi devices, electromagnetic interferences, intentional or unintentional, are more and more serious. As an example, the proliferation of ultra-wideband (UWB) devices intended to be mass-marketed to the public could cause harmful interference to GPS.

Signal loss GPS is a line-of-sight sensor, and therefore GPS measurements are subject to signal outages. If it cannot “see” four satellites, then it will not produce the expected output. This case is called signal loss. It may include the following scenarios:

• Urban environments with all buildings (the so-called urban canyons). • Inside parking structures. • In a long tunnel without any relay station. • Under heavy foliage. • Under bridges.

Hardware Malfunction A GPS receiver hardware malfunction can be caused by any abnormality of its components, such as antenna, amplifier, reference oscillator, frequency synthesizer, wire disconnection, and power lost, resulting to no output, or provide an unstable or incorrect signal. Compared to the other sources of GPS faults, the probability of a hardware malfunction of the GPS receiver is rather low and can be considered negligible. Taking all the above into account, the GPS faulty model can be described as in Figure 3.

Figure 3: GPS Faulty Model Inertial measurement unit (IMU) faulty model A low-cost IMU can output the vehicle accelerations and angular rate which can then be integrated by an Inertial Navigation System (INS) to obtain the vehicle position, velocity, and attitude. The advantage of an INS is low sensitivity to high-frequency noise and external conditions. But the measurement error of INS will accumulate if it is not calibrated on-line. Several IMU error models have been derived (Kim, Lee, Choi & al., 2003), which are all

Environmental interference

Typical error

source

Signal loss

Real world

Position information

Random noise

GPS receiver

equivalent in the Kalman filtering context. Error state vectors consist of navigation parameters, and accelerometer and gyroscope error states (Jekeli, 2001). In this model, the errors modulated by the Earth’s spin rate are neglected because of short-range nature of our present application. In this paper, the error or faulty model used is presented in figure 4 and the scenarios driving the IMU to a faulty state are discussed below: IMU Error Sources and Faulty Scenarios For this sensor, we find the following sources:

• Bias due to bearing torques (for momentum wheel types), drive excitation feedthrough, electronics offsets and environmental temperature fluctuations. Intuitively, bias is any nonzero sensor output when the input is zero.

• Scale factor error, often resulting from aging or manufacturing tolerances.

Figure 4: IMU Faulty Model Diagram

• Alignment errors: Most stand-alone IMU implementations include an initial transient period for alignment of the gimbals (for gimbaled systems) or attitude direction cosines (for strapdown systems) with respect to the navigation axes. Errors remaining at the end of this period are the alignment errors. These include tilts and azimuth reference errors. Tilt errors introduce acceleration errors through the miscalculation of gravitational acceleration, and these propagate primarily as Schuler oscillations plus a non-zero-mean position error approximately equal to the tilt error in radians times the radius from the earth center. Initial azimuth errors primarily rotate the system trajectory about the starting point, but there are secondary effects due to coriolis accelerations and excitation of Schuler oscillations.

• Cross coupling error (non-linearity). • Quantization error, which is inherent in all digitized systems. • Fault due to one or multiple of the moving parts wear out or jam, or gimbals lock.

The IMU faulty model can be illustrated as in figure 4 Odometer faulty model An odometer is one of the most common devices used for tracking and to provide relative positioning information of vehicles. In the transmission-based odometer, the distance to be determined is based on the number of counts for the wheel and calibration constants which are proportional to the radius of the tire. Thus any potential trends that change the radius and the number of counts can drive the odometer to a faulty output. Hence, in term of calibrating the

Alignment error

Scale factor error

Quantization error Real

world

Nonlinearityerror

IMU

odometer, the scale factor error is the most critical element, as it affects the total distance traveled and the forward speed of the vehicle. Hence, adequate modeling for this error state could be either a random constant or a first order Gauss-Markov process. Tire radius change The detailed sources of tire radius variation are listed in the following reference (Lezniak, Lewis & McMillen, 1977). Mainly,

• Tire radius tends to increase as vehicle velocity increases because of increasing centrifugal force on the tire.

• Tire radius tends to increase as air pressure within the tire increases due to increase tire temperature or other factors.

• Tire radius tends to increase as tread is worn off during the lifetime of the tire. Road Situation These kinds of error sources depend on the road situation, including:

• Running over objects on the road, slips or skids involving one or more wheels when the vehicle accelerates or decelerates too rapidly or travels on a snowy, icy, or wet road.

• In sharp turns, the contact point between each wheel and the road can change, so that the actual distance between the left and right wheels will be different from the one used to derive the heading.

Gears Tooth Lost An odometer can operate by counting the pass of teeth or tabs of the ferrous wheel mounted on the rotating shaft of the vehicle. If one or more teeth are lost, then the value will abate n/t, where n is the number of the lost teeth, t is the total teeth of the wheel. In the real situation, the occurrences of losing three or more teeth are so puny that they can be omitted.

In brief, the wear out of the tire, the pressure of the tire, the velocity of the vehicle, the slippage of the tire, and the gear teeth lost will all contribute to the imperfect operation of the odometer.

Inclinometer faulty model The error sources of the inclinometer may consist of:

• Error caused by thermal expansion or temperature changes. A normally distribution band-limited white noise is used to demonstrate the thermal noise.

• Drift, calibration error or quantization error due to analog to digital converter resolution.

• Electromagnetic interference (the major component of the inclinometer faulty model). It can be a uniform or a Gaussian distribution, or a combination of both. The variance and amplitude depend on the traveling environment.

• Power lost or hardware malfunction: A permanent fault, but since it is only in a very low possibility, it is omitted in this simulation.

Magnetic compass faulty model (fluxgate compass) A magnetic compass is an inexpensive absolute direction sensor. The main drawback with this device is that the quantity measured, i.e. the intensity and direction of the magnetic field, can be

distorted in the presence of metals and other electrical or magnetic fields, such as power lines, transformers and cars’ powertrain systems.

Compass operations include the following error sources (Zhao, 1997): • Hilly road error: When the vehicle is traveling over a hilly road, the compass plan will

not be parallel to the plane of the Earth surface. The compass measures only the projection of the vector components. This is a short-term magnetic anomaly.

• Random noise error: a) In the situation of traveling nearby power lines, big trucks, steel structures (such as freeway underpasses and tunnels), reinforced concrete buildings, or bridges (short-term magnetic anomalies); b) In an environment of electrical or magnetic noise, or magnetization of the vehicle body (long-term magnetic anomalies).

• Calibration error: Misalignment of the compass with respect to the vehicle frame simply results in a constant error. This type of error can also be attributed to an inaccurate estimation of the current declination.

• Permanent fault: power lost or interface cable disconnected (very low possibility, they are being omitted in the simulation).

Adaptive and robust data fusion architecture In positioning navigation systems, high precision and reliability with low cost are always pursued. Actually, for road navigation, the benefits of the information obtained by the fusion process make it possible to use multiple less powerful, lower cost sensors to achieve as good a performance as those much more expensive ones.

Figure 5: Adaptive Sensor Fusion System Kalman filter and its derivatives, the most popular data fusion methods, have been used extensively in autonomous or assisted navigation system for several years. But almost all of these applications are based on the assumption that all sensed data are complete and reliable. If anyone or more sensors are faulty, then the fusion filters will tend to malfunction as well. In order to ensure a reliable positioning estimate in the case of faulty sensors, an adaptive approach is proposed as in Figure 5.

As most of the navigation systems, the objective of the above system is to make the position estimate of the vehicle as accurate and reliable as possible. Sensors (GPS, IMU, odometer, inclinometer and compass) data which are used to compute the position and attitude of the vehicle, often involve sources of uncertainties. Meanwhile, a state space model can be constructed from the vehicle dynamic to perform the function of sensor fusion. Both their outputs (measurements and estimates) can be combined together through a particular function so as to generate a residual signal. Passing this signal through a detection process, a decision is made:

Sensors Fusion Process

Residual Generator

output

Gaussian noise

residuals Fault Detection Process

estimates measurements

either the system is running properly or there is a fault occurring, which leads to the fusion process rerunning to optimize the position estimates.

From the diagram above, we can see how a residual signal generator and a fault detector are embedded into the conventional data fusion architecture. A Kalman filter approach is used, since it is simpler and more effective to attain the residual signal via state estimation. The key idea is to reconstruct the outputs of the process with the aid of a Kalman filter and to use the estimation error, or some particular statistical functions of them to assess the residual signal.

Then the faultiness can be detected by considering the residual properties against some threshold. If any information from the sensors is detected to be faulty (residual properties go over a given threshold), then the corresponding measurements are discarded. In that case, the position estimate needs to be re-updated. In this approach, fault detection and data fusion are combined into a single Kalman architecture to construct a fault tolerance, robust and adaptable vehicle positioning system.

The detailed flow chart is shown in Figure 6. Note that the shaded blocks are part of the fault detection process. Detailed tests results presented in (Guopei & Gingras, 2007) show that the proposed Kalman filter based state estimation scheme ensures that the position estimate is always optimal and brings significant benefit to the data fusion system compare with the conventional fusion architecture without fault detection, in particular for the frequent GPS signal loss case. Performance analysis and more details on this architecture can be found in (Guopei & Gingras, 2007).

Figure 6: Adaptive Sensor Fusion Architecture Flow Chart

N

Y Decision making Thresholdri > ?

Residual generation

( )22 ˆ

ˆ

kkkk

kkk

xHzz

xHzr

−+

−=

Discard ith measurement

0=iK

Update estimate with measurement zk )ˆ(ˆˆ −− −+= kkkkk xHzKxx

Initialize −kx̂ and −

kP

Compute the Kalman gain 1)( −−− += k

Tkkk

Tkkk RHPHHPK

Update the error covariance ( ) −−= kkkk PHKIP

Project the error covariance ahead k

Tkkkk QPP +=−

+ φφ1

Project the state ahead

kkk xx ˆˆ 1 φ=−+

Next iteration

COLLABORATIVE DATA FUSION SYSTEMS So far we have considered the case of a single vehicle only. In real life however, cars are not alone on the road. We may therefore ask ourselves the following questions: can we take advantage of the other vehicles proximity and positioning data to improve our own position and, if yes, how? The intelligent vehicle systems (IVS) envisioned here would be able to communicate their position and navigation information through inter-vehicle communications (IVC). IVS will be evolving in mobile ad hoc networks called (MANET) that give access to valuable real time data, especially high precision positioning information. However, in terms of global navigation systems (GNS), individual vehicles in a given MANET would not have access to the same constellation of satellites. And certainly vehicles with good lines of sight (LOS) have more precise position estimates than vehicles with a poor LOS. Moreover, some vehicles may possess high precision DGPS or beacon-based positioning information to share in the network.

In this section, we briefly present a collaborative positioning concept that uses some of the above IVC features, along with additional range measurement capabilities, to ameliorate positioning estimates of neighboring vehicles in a MANET (Abdessamie & Gingras, 2008). Two vehicles at different locations can have different sets of visible satellites and by mean of inter-vehicle communications, the satellite information can be shared between vehicles (Berefelt, Boberg & al., 2004). Many research activities are being conducted in IVS collaborations. We ref to (Nirupama, 2005) for a good introduction to IVS research and development. For a little more technical reference on the subject and still worth reading, see (Jurgen, 1998). Designing a collaborative system for accurate estimation of relative positions of neighboring vehicles based on real-time exchange of individual GPS coordinates, while using vehicle-to-vehicle radio communications is a challenging task (Kukshya, Krishnan & Kellum, 2005). Research in the domain of collaborative navigation was usually aimed for collaborative driving systems (CDS), where car platoons or mobile robots are involved (Gaubert, Beauregard & al, 2003, Crawford & Cannon, 2005). However, most CDS previously reported require either costly vehicle-to-vehicle relative dependence or vehicle-to-infrastructure dependence.

In a recent paper (Abdessamie & Gingras, 2008) we have proposed a less expensive navigation approach, exploiting existing positioning resources embedded in vehicles and independent of road infrastructures. To illustrate the concept here, we will treat only the case of three vehicles, as shown in figure 7, which, although limited, is sufficient to understand the underlying principle in a more general case. In order to consider the design of collaborative localization architectures, such as the one we present in (Abdessamie & Gingras, 2008), we need to consider a set of various assumptions in order to simplify the model. In our case, we supposed the following conditions to be valid:

• All vehicles in the MANET are equipped with necessary navigation items (GPS, DR, IVC sensors…etc.)

• All vehicles have range measurement radars, to provide precise inter-vehicle distance. Millimeter wave radars MMW for automotive, studied in [3], would be appropriate solution to our application for they have a high Doppler sensitivity and a 200 meters range with a good precision which is very suitable for our case.

• No vehicle dynamics are considered in the CPA, we instead used a simple motion model.

• Error covariance matrix on position, heading, and inter-vehicle distance contains the global errors of vehicle systems.

• No special vehicle frame is considered

• Coordinate reference system is geodetic altitude, latitude and azimuth. • No altitude difference is considered, this corresponds to the case where all three

vehicles are located on a relatively flat plane with a constant altitude. • Inter-vehicle communications are real time and safe.

Figure 7: Collaborative positioning architecture: In this simplified example, the vehicle at far right and at far left have a good line of sight (LOS) and can share and provide their good GPS

position estimate to the vehicle in the center who has a bad LOS and therefore a poor GPS position estimate. All vehicles are assumed equipped with GPS receivers.

Collaborative uncertainty minimization In a distributive, collaborative approach, each vehicle has its own set of position estimates as well as other information such as range information from other neighboring vehicles.

To achieve a collective improvement of position estimates, we have considered a geometric data fusion approach (Abdessamie & Gingras, 2008). This method is based on the geometric analysis of the sensing uncertainty and is motivated by the geometric idea that the volume of the uncertainty ellipsoid should be minimized, as illustrated in figure 8. The resulting fusion equations coincide with those obtained by Bayesian inference, Kalman filter theory, and by weighted least-squares estimation (Abidi & Gonzales, 1992). When systematic error or bias is made small, the uncertainty ellipsoid encloses a region in space where the true value most likely exists. The center of the ellipsoid is the mean of the measurement and the ellipsoid boundary represents a distance of one standard deviation from the mean.

Geometric data fusion has been used in many research applications (Elliott, Langlois & Croft, 2001, Lee & Ha, 2001). It had proven to be a powerful uncertainty management data fusion

technique. In (Abdessamie & Gingras, 2008), we showed that a collaborative approach can be used in navigation systems and further improve the position estimate over a conventional data fusion approach. More mathematical details and performance analysis can be found in the paper.

Figure 8: Principle of geometric data fusion approach in collaborative positioning of vehicles using GPS as shown in Figure 7. The far right and far left vehicles are sharing their good GPS

position estimates using appropriate fusion in order to reduce the uncertainty ellipse of the vehicle in the center of the platoon.

CONCLUDING REMARKS AND FUTURE TRENDS In this paper, we have surveyed the positioning estimation problem applied to land navigation systems and reviewed some of the various sensor fusion techniques usually encountered in such systems. We have discussed their relative performance and limitations. The extended Kalman filter (EKF) in data fusion centralized architectures remains a design of choice for most applications. We have explained how to make such systems more robust by detecting and identifying sensor faults. Finally we looked at the possibility to exploit the presence of several vehicles in the vicinity, in order to improve one’s own position estimate using a collaborative and geometric data fusion approach.

A main trend can be seen in the field of dense sensor networks, where multilateration can render good location accuracy despite significant errors in range estimates between sensors. It can therefore be useful for vehicle navigation. This has launched a research area known as sensor location using wireless telecommunication techniques, which seeks to process potentially enormous quantities of data collectively to achieve optimal positioning results. Some papers (Biswas & Ye 2004, Hightower & Boriello, 2001) provide good surveys of available techniques for sensor location. Usually, the techniques achieving the best performance process the input data in a centralized fashion; however distributed versions are also available. Savvides, Park & Srivastava (2002) solve a global nonlinear optimization problem for localizing sensors through Kalman filtering, which yields good results, but requires a high number of anchor nodes. The multi-dimensional scaling technique employed by (Shang, Rumi & al., 2003) for sensor location achieves good results with few anchor nodes, but requires high radio connectivity. In (Savarese,

Rabaey & Beutel, 2001) good results are presented, based on an architecture employing a two-phase initiation and refinement approach, and, as opposed to the aforementioned papers, deals with relatively high levels of noise.

According to results previously reported, the algorithms achieving the best performance in sensor location techniques use quadratic constraints to minimize a linear objective function (Biswas & Ye, 2004, Doherty, Pister & Ghaoui, 2001, Hightower & Boriello, 2001). Since some of the constraints are non-convex, the papers above differ primarily in their relaxation approaches to render the problem convex. According to those publications, the solution provided by Biswas et al. (2004) has greater applicability and yields better results than the one by Doherty & al. (2001). The paper by Gentile, (2005) follows their same centralized approach, maintaining the efficiency of convex optimization. However by applying linear triangle inequality constraints as opposed to quadratic ones, the problem is automatically convex. As a result, a tighter solution is guaranteed since no relaxation of the constraints is needed.

With the current evolution of automotive technologies, embedded positioning systems will become more and more easily available at a lower cost, thus allowing all vehicles to be equipped with such technologies. In addition, all vehicles are becoming networked and equipped with wireless communication capabilities, thus allowing the use of distributed and collaborative techniques for navigation and positioning. Wireless communications networks are becoming attractive to localize vehicles using various radio-based range technologies such as received signal strength indicators (RSSI), power signal attenuation or time-of-arrival (TOA) techniques (Parker & Shahrokh, 2006, Kukshya, Krishnan & Kellum, 2005, Chan, Tsui, So & Ching, 2006, Tsukamoto, Fujii, Itami. & Itoh, 2001, Qi, Kobayashi & Suda, 2006). Originally, most existing radio-based ranging technologies for position estimation has been applied to fixed wireless sensor networks, because it is not possible, for practical and economic reasons, to equip all sensor nodes in a network with GPS receivers. Previously proposed radio-based ranging approaches have been either distributed or centralized, providing a more or less precise location estimate of the nodes, depending on the scheme used (also called course grain and fine grain schemes). Course grain localization, usually configured in a centralized fashion, requires less radio connectivity between nodes but needs a central processing node in the network to establish localization of the remaining nodes. It works fine only if a rough estimate of a node’s position is sufficient (Doherty, Ghaoui, & Pister, 2001). Conversely, fine grain localization requires more resources and radio traffic, but provides a relatively accurate node position estimates. Fine grain localization may operate in either a distributed or centralized approach. The main challenges in using these techniques for vehicular applications, originally developed for fixed sensor networks, are related to the continuous high mobility of the nodes.

In a more theoretical context, extensions of the Bayesian framework, which copes with the different types of imperfections of the various sensors and sources of information, such as imprecision, uncertainty, incompleteness, and ignorance, have also been proposed for the data fusion problem in vehicle localization. For example, Fisher’s fiducial probability, fuzzy logic, credal inference, Demster’s belief functions and transferable belief model (TBM) are being used and described in various papers, such as (Caron & Smets, 2005, Caron, Duflos & al., 2006, Smets & Ristic, 2004) to name a few.

Finally, in a more global context, the advent of new satellite networks for navigation in the years to come, such as GALILEO in Europe, are likely to boost vehicle localization worldwide capabilities and provide more flexibility in the design of future positioning systems for automotive applications (Vejrazka, 2007). Clearly, more robust, networked and distributed

intelligent positioning systems are likely to be encountered in future positioning and navigation systems.

ACKNOWLEDGMENTS I would like to thank the Canadian NCE AUTO21 for funding this research project, the University of Calgary for providing the acceleration data, Geomatic Canada for the topographical information and E. A. Wan, A. D. R. Merwe and OGI School of Sciences for the Rebel Toolkit. I would like also to thank my former students Mathieu St-Pierre, Guopei Liu and Rabah Abdessamie for their original contributions in this area. REFERENCES Abbott, E. & Powell, D. (1999). Land-Vehicle Navigation Using GPS. IEEE Proceedings of the Institute of Electrical and Electronics Engineer. (87)1, (pp. 145-162). Abuhadrous, I., Nashashibi, F., Laurgeau, C. & Chinchole, M. (2004, June), Multi-Sensor Data Fusion for Land Vehicle Localization Using MAPS TM. Paper presented at the IEEE Intelligent Vehicle Conference, Columbus, Ohio, USA. Abdessamie, R. & Gingras, D. (2008). A collaborative navigation approach in intelligent vehicles. Paper presented at the Society of Automotive Engineering (SAE) Conference, paper no 1249 Detroit USA. Abidi, M. A. & Gonzalez, R.C. (1992). Data Fusion In Robotics And Machine Intelligence, Academic Press, NY, USA. Anderson, B. & Moore, J. (1979). Optimal Filtering. Prentice Hall, Englewood, NJ, USA. Andrieu, C. & Doucet, A. (2002), “Particle filtering for partially observed Gaussian state space models,” Journal Royal Statistical Society B. (64)4, (pp. 827–836). Arulampalam, S., Maskell, S., Gordon N. & Clapp T. (2002), A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking. IEEE Transactions on Signal processing. Vol. 50, No. 2 (pp. 174-188). Beattie, J. B.. (1998). Self-localisation in the senario autonomous wheelchair. Journal of Intelligent and Robotic Systems, 22:255-267. Berefelt, F., Boberg, B, Nygards, J., Stromback, P. & Wirkander, S.-L., (2004). Collaborative GPS/INS Navigation in Urban Environment, Swedish Defence Research Agency ION-NTM FOI, (pp. 1114-1125), Sweden. Biswas, P. & Ye Y., (2004, April). Semidefinite Programming for Ad Hoc Wireless Sensor Network Localization, Presented at the IPSN-04 Conference on Information Processing in Sensor Networks, Berkeley, CA, USA, (pp. 46-54). Bonnifait, P., Bouron, P., Meizel, D. & Crubille, P. (2003). Dynamic Localization of Car-like Vehicles using Data Fusion of Redundant ABS Sensors. The Journal of Navigation. UK, (56)3, (pp. 429–441). Brown, R. G. & Hwang, P. C. (1997). Introduction to Random Signals and Applied Kalman Filtering. John Wiley and Sons, New York, NY, USA. Caron, F., Smets, P., Duflos, E. & Vanheeghe, P. (2005, July), Multisensor data fusion in the frame of the TBM on reals. Application to land vehicle positioning. Paper presented at the International Conference on Information Fusion (FUSION'05), Philadelphia, PA, USA

Caron, F., Duflos, E., Pomorski, D. & Vanheeghe, P. (2006), GPS/IMU Data Fusion using Multisensor Kalman Filtering: Introduction of Contextual Aspects. Information Fusion, 7(2), (pp. 221-230), Elsevier, Holland. Caron, F., Davy, M., Duflos, E. & Vanheeghe, P. (2007) Particle Filtering for Multisensor Data Fusion with Switching Observation Models: Application to Land Vehicle Positioning. IEEE Transactions on Signal Processing. (55)6, (pp. 2703-2719). Cannon, M. E., Nayak, R., Lachapelle, G., Salychev, O. S. &Voronov, V. V. (2001) Low-cost INS/GPS Integration: Concepts and Testings, Journal of Navigation, 54(1), (pp. 119-134). Chan, Y-T, Tsui, W-Y., So, H-C. & Ching, P-C. (2006), Time-of-Arrival Based Localization Under NLOS Conditions. IEEE Transactions on Vehicular Technology, (55)1, (pp. 17-24). Chuah, T. C. & Sharif, B. S. (2001) Robust adaptive spread-spectrum receiver with neural-net preprocessing in non-gaussian noise. IEEE Transaction on Neural Networks, (12)3, (pp 546-558). Crawford, S. & Cannon, M.E. (2005). Performance Evaluation of Sensor Combinations for Mobile Platoon Control; Positioning, Location, and Navigation (PLAN) Group, (Tech. Rep.) University of Calgary, Calgary Alberta, Canada Crisan, D. & Doucet, A. (2002). A survey of theoretical results on particle filtering for practitioners,” IEEE Transaction on. Signal Processing, (50)3, (pp. 736–746). Doherty, L., Pister, K.S.J. & El Ghaoui, L. (2001, April). Convex Position Estimation in Wireless Sensor Networks. Paper presented at the INFOCOM 2001. 20TH Annual Joint Conference of the IEEE Computer and Communications Societies. Proceedings (pp. 1655-1663). Doherty, L., Ghaoui, L. E. & Pister, K. (2001). Convex position estimation in wireless sensor networks. Paper presented at the INFOCOMM Conference. Anchorage, Alaska, USA, (3), (pp. 1655-1663). Doucet, A. & Wang, X. (2005), Monte Carlo methods for signal processing: A review in the statistical signal processing context. IEEE Signal Processing Magazine, (22)6, (pp. 152-170). Drane, C. & Rizos C. (1997). Positionng systems in intelligent transportation systems. Artech House, Boston, Ma., USA. Durant-Whyte, H. (1994), Where am I ? Industrial Robot, Vol. 21 No. 2,(pp. 11-16), MCB University Press Elliott, J. D., Langlois, D. & Croft E. A. (2001, June) Sensor Uncertainty Management for an Encapsulated Logical Device Architecture: Part I. American Control Conference, Arlington, USA, Proceedings (pp. 4282-4387). Farrell, J. & Barth, M. (1999). The Global Positioning System and Inertial Navigation. NY, USA McGraw-Hill. Gaubert, N.; Beauregard, M.; Michaud, F. & de Lafontaine, J. (2003, October). Emulation of Collaborative Driving Systems Using Mobile Robots. Presented at the IEEE International Conference on Systems, Man and Cybernetics. Washington DC, USA, Proceedings Vol.1, (pp.856 – 861). Gelb, A., (1974). Applied Optimal Estimation". The MIT Press, Cambridge, Massachusetts, USA 1974. Gentile, C. (2005, May), Sensor Location through Linear Programming with Triangle Inequality Constraints. Paper presented at the ICC-IEEE International Conference on Communications, Proceedings Vol 5, (pp 3192-3196). Grewal, M. S., Weill, L. R. & Andrews, A. P., (2001). Global Positioning Systems, Inertial Navigation, and Integration, John Wiley & Sons, New York, NY, USA.

Guopei, L. & Gingras, D. (2007, April). Adaptive and Reconfigurable Data Fusion Architectures in Vehicle Positioning Navigation Systems. Paper presented at the Society of Automotive Engineering (SAE) conference. Paper no. 1108, Detroit, USA. Hagan, M. T. & Menhaj, M. (1994) Training feedforward networks with the Marquardt algorithm. IEEE Transactions on Neural Networks, (5)6, (pp. 989-993). Hashimoto, M., Kawashima, H., Nakagami, T. & Oba F. (2001, October). Sensor Fault detection and Identification in Dead-Reckoning System of Mobile Robot: Interacting Multiple Model Approach. Paper presented at the IEEE/RSJ International Conference on Intelligent Robot and Systems, Mani, Hawaii, USA. Hightower, J. & Borriello, G. (2001, August) Location Systems for Ubiquitous Computing, IEEE Computer, (34)8, (pp. 57-66). Isermann, R., (2005). Fault-Diagnosis Systems: An Introduction-From Fault Detection To Fault Tolerance, Springer Verlag, NY, USA. Jekeli, C. 2001, Inertial Navigation Systems with Geodetic Applications, De Gruyter, (pp.308-318), New York, USA Julier, S. (1997), Process Models for the Navigation of High-Speed Land Vehicles. Wadham College, Oxford, UK, pp. 184. Julier, S. J. & Uhlmann, J. K. (1997), A New Extension of the Kalman Filter to Nonlinear Systems. Paper presented at the 11th International Symposium on Aerospace/Defense Sensing, Simulation and Controls. Orlando, FL, USA Jurgen, R. K. (1998). Navigation and Intelligent Transportation Systems. Progress In Technology, Automotive Electronics Series. SAE International, Detroit, USA. Kalman, R. E. (1960), A new approach to linear filtering and prediction problems, Transactions of the American Society of Mechanical Engineering (ASME). (82) (Series D), pp. 35-45. Kim, S.-B, Lee, S.-Y.,. Choi J. & al. (2003). Multi-Sensor data fusion for future telematics applications. Journal Astronomical Space Sciences 20(4), pp.359–364. Knight, D. T., (1997). Rapid development of tightly-coupled GPS/INS systems. IEEE Aerospace and Electronic Systems (AES) Magazine, 12(2), (pp.14-18). Kobayashi, F. & Arai, F. (1998). Sensor fusion system using recurrent fuzzy inference. Journal of Intelligent and Robotic Systems, (23), 201-216 . Kronander, J. (2004, December), Robust Automotive Positioning: Integration of GPS and Relative Motion Sensors, PhD Dissertation, Linköping, Sweden. Kukshya, V.; Krishnan, H. & Kellum, C. (2005, June). Design of a System Solution for Relative Positioning of Vehicles Using Vehicle-to-Vehicle Radio Communications During GPS Outages. Paper presented at the IEEE Vehicular technology Conference, Dallas, Tx, USA, vol. 2, (pp. 1313-1317). Laneurit, J., Blanc, C., Chapuis, R. & Trassoudaine L. (2003, June). Multisensorial data fusion for global vehicle and obstacles absolute positioning. Paper presented at the IEEE Intelligent Vehicle Conference, San Francisco, USA. LeCunn, Y. & Bottou, L. (2001). Gradient-Based Learning Applied to Document Recognition. IEEE Press, NY, USA. Lee J. & Ha, I., (2001). Real-Time Motion Capture for a Human Body using Accelerometers. Robotica, vol. 19, (pp. 601–610). Cambridge University Press, Cambridge, UK. Lezniak, T. W., Lewis, R. W. & McMillen, R. A. (1977). A Dead Reckoning/Map Correlation System for Automatic Vehicle Tracking. IEEE Transactions on Vehicular Technology, (VT-26)1, (pp 47-60).

Li, W. & Leung, H. (2003). Constrained Unscented Kalman Filter Based Fusion of GPS/INS/Digital Map for Vehicle Localization. Paper presented at the IEEE International Conference on Intelligent Transportation Systems, Shangai, China. Magrabi, S. M. & Gibbens, P. W. (2000, June). Decentralized Fault Detection and Diagnosis in Navigation Systems for Unmanned Aerial Vehicles. Paper presented at the IEEE Position Location and Navigation Symposium, San Diego,USA. Mao, X., Wada, M., Hashimoto, H., Saito, M. & Suzuki, I. (2003, November). EM/Unscented Smoothing Based Parameter Learning for Nonlinear Models for GPS Positioning. Paper presented at the 10th World Congress on Intelligent Transport Systems, Madrid, Spain. Merwe, A. D. R., Freitas, N. & Wan, E. (2000, August). "The Unscented Particle Filter, Tech report CUED/F-INFENG/TR-380", Cambridge University Engineering Department, Cambridge, England. Nirupama, B. (2005). Wireless Sensor Networks, A Systems Perspective, 2005 Artech House, Boston, USA Parker R., Shahrokh V. (2006, September), Vehicle Localization in Vehicular Networks. Paper presented at the IEEE 64th Vehicular Technology Conference VTC-2006, Melbourne, Australia. Patton, R., Frank, P. & Clark, R. (1989). Fault Diagnosis in Dynamic System: Theory and Applications, Prentice Hall, USA. Pouliezos, A. D. & Stavrakakis, G. S. (1994), Real Time Fault Monitoring of Industrial Processes, Kluwer Academic Publishers, NY, USA. Qi, Y., Kobayashi, H. & Suda, H. (2006). On Time-of-arrival Positioning in a Multipath Environment. IEEE Transactions on Vehicular Technology, (55)5, (pp. 1516-1526). Redmill, K. A., Martin, J. I. & Özgüner, U. (2000, October). Sensor and data fusion design and evaluation with a virtual environment simulator. Paper presented at the IEEE Intelligent Vehicles Symposium. Dearborn, Mi, USA, Proceedings (pp. 668-674). Redmill, K. A., Kitajima, T. & Özgüner, U. (2001, June) U., DGPS/INS Integrated Positioning for Control of Automated Vehicle. Paper presented at the IEEE Intelligent Transportation Systems, Oakland, USA Roumeliotis, S. & Bekey, G. (2000, April). Bayesian estimation and Kalman filtering: A unified framework for mobile robot localization. Paper presented at the IEEE International Conference on Robotics & Automation, San Francisco, CA, USA. Savarese, C., Rabaey, J.M. & Beutel, J. (2001, May), Location in Distributed Ad-Hoc Wireless Networks. Paper presented at the IEEE Conference on Acoustics, Speech, and Signal Processing, Salt Lake City, Utah, USA, Proceedings (pp. 2037-2040). Savvides, A., Park, H. & Srivastava, M. B (2002, September). The Bits and Flops of the N-hop Multilateration Primitive For Node Localization Problems. Paper presented at the ACM conference on Wireless Sensor Networks and Applications, WSNA’02, Atlanta, Georgia, USA Proceedings (pp. 112-121). Shang, Y., Rumi, W., Zhang, Y. & Fromherz, M. P. J. (2003, June). Localization from Mere Connectivity. Paper presented at the ACM conference on Mobile Ad Hoc Networking and Computing. Annapolis, Maryland, USA, Proceedings (pp. 201-212). Smets, P. & Ristic, B. (2004, April) Kalman filters and joint tracking and classification in the transferable belief model framework. Paper presented at the 7th International Conference on Information Fusion, (pp. IF04–0046), Stockholm, Sweden. St-Pierre, M. & Gingras, D. (2005, June). Comparison between the unscented Kalman filter and the extended Kalman filter for the position estimation module of an integrated navigation

information system. Paper presented at the Institute of Electrical and Electronics Engineering (IEEE) Intelligent Vehicles Conference, Parma, Italy. St-Pierre, M. & Gingras, D. (2004, March), Neural network based data fusion for vehicle positioning in land navigation system. Paper presented at the Society of Automotive Engineering (SAE) conference. Paper no. 345, Detroit, USA. Suresh, B. & Sivan, K. (2004, April). Performance evaluation of multi-sensor data-fusion systems in launch vehicles. Sadhana ,Vol. 29, Part 2, ( pp. 175–188), India Tsukamoto, K, Fujii, M., Itami, M. & Itoh K. (2001, May), Study on acquisition and tracking of vehicle location using IVC and RVC dynamic networks. Paper presented at the IEEE Intelligent Vehicle Symposium, (pp. 467-470), Tokyo, Japan. Vejrazka, F. (2007, April). Galileo and the Other Satellite Navigation Systems. Paper presented at the IEEE 17th Radioelektronika International Conference. Wan, E. A. & Merwe, R. V. D. (2000). The Unscented Kalman Filter for Nonlinear Estimation. Paper presented at the Symposium on Adaptive Systems for Signal Processing, Communication and Control (AS-SPCC-2000), Lake Louise, Alberta, Canada. Wu, Q., Gao, Z. & Wan, D. (2002, April). An adaptive information fusion method to vehicle. Paper presented at the IEEE Position location and navigation symposium. Proceedings (pp. 248-253). Zhao, Y. (1997). Vehicle Location and Navigation Systems. Artech House, Norwood, USA