Ieee Paper - 1

544 IEEE Transactions on Consumer Electronics, Vol. 58, No. 2, May 2012

Contributed Paper Manuscript received 12/02/11 Current version published 06/22/12 Electronic version published 06/22/12. 0098 3063/12/$20.00 © 2012 IEEE

Vehicle Navigator using a Mixture Particle Filter for Inertial Sensors/Odometer/Map Data/GPS Integration

Jacques Georgy, Member, IEEE, Aboelmagd Noureldin, Senior Member, IEEE, and Chris Goodall

Abstract — The market for vehicular navigators boomed over

the last few years. These navigators rely mainly on satellite based navigation systems such as the Global Positioning System (GPS) to assist drivers. Due to interruption or degradation in such systems in dense urban scenarios, they have to be augmented with other systems to achieve continuous and accurate vehicular navigation. GPS is integrated with low-cost micro-electro mechanical system (MEMS)-based inertial sensors. However, these sensors provide inadequate performance in degraded GPS environments because of their complex error characteristics that often lead to large position drift errors. This paper proposes a continuous and accurate solution integrating low-cost MEMS-based inertial sensors, the vehicle odometer, GPS, and map data from road networks. Despite the traditional inadequate performance of MEMS-based sensors in this problem, the performance is enhanced through: (i) a special combination of inertial sensors and odometer that has better performance for land vehicles than traditional solutions; (ii) The use of map information from road networks to constrain the positioning solution; (iii) The use of an advanced particle filtering (PF) technique to perform the integration, which work with nonlinear models and better modeling of inertial sensor errors, in addition to better integration with the map data. The performance of the proposed positioning system has been verified extensively on real road tests in downtown trajectories with degraded or totally denied GPS for long durations1.

Index Terms — Land Vehicle Navigation, Inertial Sensors, GPS, Particle Filter, Map Matching.

I. INTRODUCTION

The interruptions and degradations in Global Navigation Satellite Systems (GNSS)-based vehicular navigation solutions in dense urban scenarios such as urban canyons and tunnels lead to the fact that these solutions have to be augmented with other systems to achieve continuous and accurate navigation. Although there are several works to enhance GNSS receiver capabilities and applicability on mobile navigators [1]-[3], but still augmentation with other systems is the best way toward achieving more available and more accurate navigation systems. Among the systems that can be integrated with satellite navigation are: (i) Dead reckoning systems, such as inertial

1 Jacques Georgy is with Trusted Positioning Inc., Calgary, AB T2L 2K7, Canada (e-mail: [email protected]).

Aboelmagd Noureldin is with the Department of Electrical and Computer Engineering, Queen’s University, Kingston, ON K7L 3N6, Canada, and also with the Department of Electrical and Computer Engineering, Royal Military College of Canada, Kingston, ON K7K 7B4, Canada (e-mail: [email protected]).

Chris Goodall is with Trusted Positioning Inc., Calgary, AB T2L 2K7, Canada (e-mail: [email protected]).

navigation systems (INS) [4]-[6] and odometry [7]; (ii) Systems that provide additional sources of information such as map data and road networks [8]-[12]; (iii) Other wireless systems [13]. This latter may still suffer from interruption and multipath effects since all wireless signals are subject to blockage and multipath to some degree. For land vehicle navigation, a GNSS receiver, such as a Global Positioning System (GPS) receiver, can be integrated with micro-electro-mechanical system (MEMS)-based inertial sensors because of their low cost, small size, light weight and low power consumption. Despite the advantages of these sensors, they provide inadequate performance in degraded GPS environments such as urban canyons or tunnels because of their complex error characteristics which are stochastic in nature and difficult to model especially in the absence of absolute updates.

This paper proposes a vehicular navigator based on integrating low-cost MEMS-based inertial sensors, the vehicle odometer, GPS, and map data from road networks. Despite the traditional inadequate performance of MEMS-based sensors in this problem, three methods to enhance the performance are proposed in this work: (i) The use of a three-dimensional (3D) reduced inertial sensor system (RISS) that has better performance for land vehicles than traditional full-inertial measurement unit (IMU) solutions; (ii) The use of map information from road networks to constrain the positioning solution; (iii) The use of advanced nonlinear filtering techniques based on particle filtering (PF) to perform the integration of 3D RISS/GPS/Map data. This usage of PF also enables the utilization of sophisticated models for inertial sensor stochastic drifts. Furthermore, PF has the advantage of giving the ability to tightly incorporate the map information inside the filter itself.

The first enhancement is due to the use of 3D RISS to provide a 3D position, velocity, and attitude (pitch, roll, and azimuth). This 3D RISS [14] consists of a single axis gyroscope aligned with the vertical axis of the vehicle, two horizontal accelerometers aligned with the forward and the transversal axes of the vehicle, and the vehicle odometer. The advantages of the proposed 3D RISS over a full IMU are due to two factors, namely the calculation of pitch and roll from accelerometers instead of the traditionally used horizontal gyroscopes, and the calculation of velocity from odometer-derived speed instead of accelerometers. The latter leads to another improvement caused by the projection (using the attitude angles) of the speed (in RISS case) rather than acceleration (in full-IMU case). The integrated 3D RISS/GPS solution has another advantage, which is that the higher performance of the RISS is exploited in overcoming multipath effects that contaminate the GPS signals in urban canyons.

J. Georgy et al.: Vehicle Navigator using a Mixture Particle Filter for Inertial Sensors/Odometer/Map Data/GPS Integration 545

the use of map information from the road network to constrain the positioning solution. This extra aid is beneficial during GPS degradation or blockage, to mitigate the drift of the positioning solution. The map information is not just employed as a matching or pull-back step but it is used inside the filter itself, since PF gives more flexibility because there are no restrictions on its model. Thus the integration of the RISS/GPS solution with map matching can be considered a tight integration as opposed to loose integration or the pull-back strategy, which implements a single matching step and uses it as either the new solution or as a single measurement update to the filter. This latter is commonly used in most of the currently available map-matching techniques.

The third approach for enhancing the solution is related to the state estimation technique and the advantages it gives through its capability and flexibility. The commonly used Linearized KF (LKF) and Extended KF (EKF) [4], [5] require a linearized system model for the navigation error states and linear models for the stochastic sensor errors. The traditional KF-based techniques have proven very useful for higher grade INS, but for low-cost MEMS grade these techniques suffer from divergence during outages due to approximations during the linearization process and inappropriate system modeling [15]. To enhance the performance of MEMS-based INS/GPS integration, PF is employed in this paper as a nonlinear estimation technique that does not require linearized models and can deal with nonlinear non-Gaussian models. Thus, PF [16] can accommodate arbitrary sensor characteristics, motion dynamics, and noise distributions. Different versions of PF were used in the literature for INS/GPS integration, some examples can be found in [17]-[23].

The PF used in this work employed nonlinear total-state system and measurement models. Furthermore, the capabilities of this filter were exploited through the use of sophisticated modeling for the MEMS-based inertial sensor stochastic drifts, which outperform traditional stochastic models by improving the positioning accuracy during GPS outages. Moreover, a routine that automatically detects the degraded GPS measurements is used. In addition, the PF gives the opportunity to use the map data inside the filter itself.

The proposed integrated navigation solution have additional modules for: (i) mounting misalignment estimation between the sensor assembly and the vehicle; (ii) estimating the scale factor of the vehicle odometer; and (iii) incorporating a barometer (if available) by automatically calibrating it and using it for height update to the integrated navigation solution.

II. 3D REDUCED INERTIAL SENSOR SYSTEM

A navigation solution based on KF for loosely coupled 2D RISS/GPS integration was introduced in [24] with the assumption that the vehicle mostly stays in the horizontal plane, while [25] proposed a Mixture PF for loosely coupled 2D RISS/GPS integration. 2D RISS consists of a single

gyroscope vertically aligned with the body frame of the vehicle together with the vehicle odometer. The 3D RISS was first proposed in [14] where Mixture PF was used for 3D RISS/GPS integration.

The 3D RISS uses one gyroscope, two accelerometers and the vehicle odometer to compute a 3D position, 3D velocity, and 3D attitude. The accelerometers are aligned with forward and transversal axes of the vehicle body frame; a reliable model for the Earth gravity and an odometer are used to decouple the actual acceleration of the vehicle from the accelerometers readings thus making them appropriate to calculate pitch and roll respectively. This configuration obviates the need of two, relatively costly and error prone gyroscopes. The single gyroscope aligned with the vertical axis of the vehicle body frame is used with the pitch and roll information to obtain an accurate azimuth angle in the horizontal East-North plane which is completely compensated for tilt errors. The forward speed derived from the vehicle odometer or from wheel encoders together with the pitch and azimuth angles is used to calculate the East, North and vertical (Up) velocities. Consequently, the latitude, longitude and the altitude of the vehicle are determined yielding a 3D position of the vehicle. The equations of 3D RISS are presented in [14], as well as its advantages over using a full-IMU for wheel-based mobile platforms and its advantages over 2D dead reckoning systems.

III. 3D RISS/GPS INTEGRATION

A. Problem Formulation

The aim is to estimate the state of the vehicle kx at the

current time step k, given a set of measurements

(observations) 0 ,...,k kZ z z acquired at time steps

0,1,...,k. The state of the vehicle is

, , , , , ,Tf

k k k k k k k kh v p r A x , where k is the

latitude of the vehicle, k is the longitude, kh is the altitude,

fkv is the forward speed, kp is the pitch angle, kr is the roll

angle, and kA is the azimuth angle at the kth time step.

The nonlinear state transition model (system or motion model) is given by

1 1 1, ,k k k k x f x u w (1)

where ku is the control input which is the RISS reading, and

kw is the process noise which is independent of the past and

present states and accounts for the uncertainty in vehicle motion and RISS readings. The state measurement model is

,k k kz h x ν (2)

where kν is the measurement noise which is independent of

the past and current states, and the process noise, and accounts for uncertainty in GPS readings.

These models are nonlinear as will be seen in the next subsection. There is no need to linearize them because the

The second approach to achieve an enhanced solution is


employed technique can deal with nonlinear models. When using KF, linearized models for the navigation error states are used, and only the first order terms of the Taylor series expansion are considered. This leads to using an error-state approach where the KF estimates the error in the navigation states and not the states themselves. On the other hand, the approach used in this paper is a total-state approach as there is no need for linearization. So the system and measurement models used by the integration filter are the total-state nonlinear models.

B. System and Measurement Models To describe the system model indicated in equation (1), the motion model for the navigation states, the control input and the process noise terms are first introduced. The RISS measurements provided by the odometer, the two accelerometers and the gyroscope comprise the control input;

1 1 1 1 1 1

Tod od x y zk k k k k kv a f f u where

1odkv is the

speed derived from the vehicle odometer, 1

odka is the

acceleration derived from the vehicle odometer, 1

xkf

is the

transversal accelerometer measurement, 1y

kf is the forward

accelerometer reading, and 1

zk the angular rate obtained from

the vertically aligned gyroscope, respectively. The corresponding process noise vector is

1 1 1 1 1 1

Tod od x y zk k k k k kv a f f w where

1odkv is the stochastic error in odometer derived speed,

1odka

is the stochastic error in odometer derived acceleration, 1

xkf

is

the stochastic bias error in transversal accelerometer, 1

ykf is

the stochastic bias error in the forward accelerometer, and

1zk is the stochastic bias error in gyroscope reading.

The system (state transition) model or motion model is:

1 1 11

1

1 1 11

1 1

1 1 1

1 1

1 1 1 1 1

1 1

cos cos

sin cos

cos

sin

( ) ( )sin

(sin

fk k k

kM k

fk k k

kk

N k k

fkk k k

od odkk kf

k y y od odk k k k

k

k xk

k

v A pt

R h

v A pt

R h

h v p th v vv

f f a ap

gr

fA

1 1 1 1 1

1 1 1 1 11

1

) ( )( )

cos

sin cos tantan sin

x od od z zk k k k k

k

fEe k k k k

kNN k

f v v

g p

v A pUt tU R

h

(3)

where MR and NR are the radii of curvature of the earth,

t is the sampling time, and:

1 1 1

1 1 1 1 1 1

1 1 1

1 1 1 1 1 1

sin cos cos

cos cos sin sin sin sin

cos cos cos

sin cos cos sin sin sin

E zk k k

zk k k k k k

N zk k k

zk k k k k k

U A p

A r A p r

U A p

A r A p r

(4)

1 1 1z z zk k k t (5)

Both loosely coupled integration and tightly coupled integration for RISS and GPS were implemented. In loosely coupled integration, GPS position and velocity updates are considered, thus the GPS observation vector is given as

e unTv vvh

k k k k k k kz z z z z z z which includes

the GPS latitude, longitude, altitude, and velocity components along East, North, and Up directions respectively. The measurement model can therefore be given as:

( , )sin cos

cos cos

sin

e e

n n

u u

k k k

k k kh hk k k

k k kv vfk k k k kv vfk k k k kv vfk k k k

z

z

z h

z v A p

z v A p

z v p

z h x ν

ν

ν

ν

ν

ν

ν

(6)

where e unTv vvh

k k k k k k k ν ν ν ν ν ν ν is the

noise in GPS readings. In the case of tightly-coupled integration, the measurement

model is a nonlinear model that relates the GPS raw measurements (pseudoranges and pseudorange rates) at a time

epoch k, kz , to the state kx , and the measurement noise k .

The nonlinear measurement model for tightly-coupled integration is in the form:

( , )k k kz h x (7)

where

1 1 TM Mk k k k k z (8)

1 1, , , ,

TM Mk k k k k (9)

The part of the measurement model for the pseudoranges is:

1,

,

2 2 21 1 1 1,

2 2 2

,

GPSk

M GPSk

k k k k k k r k

M M M Mk k k k k k r k

x x y y z z b

x x y y z z b

(10)

where the receiver position is

2

cos cos

cos sin

1 sin

N k k kk

k k N k k k

kN k k

R hx

y R h

z R e h

x (11)

and Tm m m m

k k k kx y z x is the position of the thm

satellite at the corrected transmission time but seen in the ECEF frame at the corrected reception time of the signal. The


part of the measurement model for the pseudorange rates is: 1,

,

1 1 1 1 1 1 1, , , , , , , , , ,

, , , , , , , , , ,

1 .( ) 1 .( ) 1 .( )

1 .( ) 1 .( ) 1 .( )

GPSk

M GPSk

x k x k x k y k y k y k z k z k z k r k

M M M M M M Mx k x k x k y k y k y k z k z k z k r k

v v v v v v d

v v v v v v d

(12)

where , , ,

Tm m m mk x k y k z kv v v v is the thm satellite

velocity in the ECEF frame, and

, , ,

T

k x k y k z kv v v v is the true receiver velocity in

the ECEF frame and thus:

, ,,

, , ,

, ,

,

,

,

sin sin cos cos cos

cos sin sin cos sin

0 cos sin

x k e ke Mech

y k k n k

z k u k

k k k k k e k

k k k k k n k

k k u k

v v

v R v

v v

v

v

v

(13)

Furthermore, the line of sight unit vector from thm satellite to receiver is expressed as follows:

2 2 2

, , ,

, ,

1 ,1 ,1

Tm m m

k k k k k kmk

m m mk k k k k k

Tm m mx k y k z k

x x y y z z

x x y y z z

1 (14)

where the receiver position is as defined above. In the case of tightly-coupled integration, the system model is

augmented with two states, namely: the bias of the GPS receiver

clock rb and its drift rd . Both of these are modeled as follows:

r br

dr

d wb

wd

(15)

where bw and dw are white Gaussian noise terms. In

discrete form it can be written as:

, 1 , 1 , 1,

, , 1 , 1

r k r k b kr k

r k r k d k

b d w tb

d d w t

(16)

This model is used as part of the system model described above.

C. Bayesian Filtering

The errors and uncertainty in sensor readings and in vehicle motion motivates the use of a probabilistic algorithm for this estimation problem. Probabilistic algorithms for estimation calculate a probability distribution instead of calculating a single best estimate.

This state estimation problem from sensor observations is an instance of the Bayesian filtering problem. Bayesian filtering has a recursive solution that uses Bayes theorem, and it yields multi-dimensional integrals. According to [16], it is impossible to evaluate this recursive solution analytically

except in a few special cases which includes linear Gaussian state space models (using KF). For nonlinear non-Gaussian systems these multi-dimensional integrals are intractable and approximate solutions are needed.

One of these solutions is the EKF, which is the traditional method to integrate INS and GPS. The EKF linearizes the system and measurement models. As mentioned in [17], while KF gives optimal solution for linear Gaussian models, it becomes suboptimal when these assumptions are violated like the problem at hand.

Another solution that avoids the linearization of the models is to handle the multidimensional integrals numerically using a Monte Carlo integration method. The solution to the Bayesian filtering problem leads to the sequential Monte Carlo methods (also called PF) [16] which propagates the probability density in the form of a set of random samples or particles.

When using LKF or EKF, the models need to be linearized which leads to the error-state approach where the KF estimates the error in the navigation states and not the states themselves. The system model used by KF is the dynamic error model which is a linearized model. Also there is a separate INS mechanization used. On the other hand, the approach used in this paper is a total-state approach which does not require linearization.

D. The Sampling/Importance Resampling (SIR) Algorithm

The description of the SIR PF can be found in [16], [26]-[28]. At time k, the probability density function (PDF)

k kp Zx is represented by a set of N random samples or

particles (1) ( ),..., Nk k kS s s distributed according to it.

( ) ( ) ( ),i i ik k ks x denotes the thi sample where ( )i

kx is

the vehicle state and ( )ik is the weight of the sample.

To initialize the filter at time k=0, the sample set

( ) ( )0 0 0, 1,...,i iS i N x (where ( )

0

1i

N ) is

sampled from 0p x which encodes any knowledge about

the vehicle’s initial state. For INS/GPS integration, the algorithm is initialized with samples drawn from a Gaussian density with mean around the GPS position and velocity.

Each iteration of the SIR algorithm consists of three steps: 1. The prediction phase: starting from the set of samples of

the previous iteration 1kS , the motion model is applied to

each sample and one sample is drawn from the prior

importance density 1 1,ik k kp x x u which is fully

specified by the system model (Equation 1) and the process

noise PDF kp w . A new sample set kS is obtained that

approximates the predictive density 1k kp Z x .

2. The update phase: the measurement kz is taken into

account and each of the samples in kS is weighted using the

observation likelihood ik kp z x which is fully specified by


the measurement model (Equation 2) and the measurement noise PDF kp ν , then all weights are normalized. The

weighted sample set kS approximates the density k kp Zx .

3. The resampling step: the equally weighted sample set

kS is obtained by resampling from the weighted set kS . The

obtained kS approximates the density k kp Zx .

In the prediction phase for RISS/GPS integration, the samples are predicted from the RISS model. Then in the update phase, the most recent GPS observation is used to adjust the importance weights of the samples obtained in the prediction stage. Then the sample set is resampled.

E. Mixture Particle Filter

This modified version of particle filtering was first reported in the area of robotics in [28], [29] and had further elaboration in [30]. In Robotics, the SIR Particle filter used for mobile robot localization is called Monte Carlo Localization (MCL), and this modified version is called MCL with planned sampling [28] or Mixture MCL [30].

In the prediction phase, the SIR PF samples from the

prior importance density 1 1,ik k kp x x u , which does

not depend on the last observation. In MEMS-based INS/GPS integration, the use of the probabilistic motion model as importance density makes the SIR PF suffers from poor performance because higher drift errors of MEMS-based sensors results in inadequate number of samples in the true PDF region. Because of this limitation of the SIR PF, it has to use a very large number of samples to assure good coverage of the state space, thus making it computationally expensive. Mixture PF is a variant of PF that aims to overcome this limitation of SIR and to use significantly less samples.

As described earlier, in the SIR PF the samples are predicted from the motion model, and then the most recent observation is used to adjust the importance weights of this prediction. This enhancement adds to the samples predicted from the motion model some additional samples predicted from the most recent observation [30]. The importance weights of these new samples are adjusted according to the probability that they came from the previous belief of vehicle state (i.e. samples of the last iteration) and the latest vehicle motion. These new samples were called planned samples in [28] or samples generated from the dual of MCL in [30].

The planned samples are drawn from the importance

density k kp z x which is the observation likelihood. These

samples are consistent with the most recent observation but ignorant of the previous belief about the vehicle state

1 1k kp Z x and the motion 1k u . The samples are weighted

using 1 1,i ik k kp x x u . The version of PF that uses this

type of sampling alone is known as likelihood PF [31].

In the version of PF used in this research and as described in [30], a number of samples (a suitably chosen ratio from the

total number of samples) are drawn from k kp z x and

added to the samples drawn from 1 1,ik k kp x x u .

Samples in these two groups are weighted with their respective weight update equations, and then the resampling is achieved. According to [30], these two importance densities have complimentary advantages and disadvantages, so their combination gives better performance. This version of PF is called Mixture Particle filter after the name used in [30], because it samples from a mixture of importance densities.

In RISS/GPS integration, some samples predicted according to the most recent GPS observation are added to those samples predicted based on the RISS model. The importance weights of these additional samples are adjusted according to the probability that they were generated from the samples of the last iteration and the motion model with the latest RISS reading. When the GPS signal is not available, only samples based on RISS are used, but when GPS is available both types of samples are used giving better performance, which leads to a better performance during GPS outages. Also adding the samples from GPS observation leads to faster recovery to true position after GPS outages. An additional advantage is better coverage of the state space and this enables the use of significantly less particles while still enhancing the performance.

F. Additional Modules for this integrated Solution

Additional modules are implemented with the above described solution to further improve the positioning quality. The first module is related to overcoming the low-cost MEMS-based inertial sensors stochastic drift by relying on advanced modeling, while the second module is aimed at overcoming the degraded GPS performance in both rural scenarios with dense canopies or downtown scenarios due to blockages, multipath or signal reflections without a direct line of sight. This second module automatically detects such degradations and can also switch to a hybrid integration mode that automatically switches between loosely and tightly coupled integration schemes to benefit from the advantages of both. The third module is for mounting misalignment estimation that helps estimating any mounting misalignment between the sensor assembly and the vehicle. The fourth module is for estimating the scale factor of the vehicle odometer that may happen because of different reasons such as changing wheel pressure and consequently the wheel diameter. If a barometer is available in the sensor assembly, a fifth module automatically calibrates the barometer and uses it for height update to the integrated navigation solution.

A nonlinear system identification technique was used for giving insight and identifying more efficient models for the stochastic sensors’ errors instead of the current linear models


used in the literature (e.g. Gauss Markov or random walk). Since the remaining main source of error caused by the inertial sensors in RISS is the vertically aligned gyroscope (this source of error is also available in a full-IMU and can be amplified by other errors), the identified models for this gyroscope stochastic drift were used inside the Mixture PF, thus exploiting its capabilities over KF-based techniques. Equations were derived to obtain measurement updates for this stochastic drift from GPS when adequate. Adequacy is decided based on some parts of the technique mentioned below.

The high performance of the Mixture PF 3D RISS solution is exploited to automatically detect GPS degraded performance which often occurs in urban and rural canyons. First, both the number of satellites and the standard deviation or the dilution of precision (DOP) are used as checks for the GPS quality. Despite these two checks, some GPS readings with degraded performance (especially because of signal reflections without direct line of sight) may still find their way to update the filter and can jeopardize its performance. Thus further checks have to be performed. The odometer and motion constraints on land vehicles are utilized in such assessments. The first check involves assessing the horizontal position update of GPS and makes use of speed derived from odometer or wheel encoder readings. The second check is for GPS altitude and exploits the accurate estimation of this state from the Mixture PF 3D RISS. The third and fourth checks are for azimuth angle updates from GPS. The third uses vehicle speed (to assure motion) and standard deviation or DOP, while the fourth uses motion constraints on land vehicles. The fifth check is for providing GPS updates for the stochastic gyroscope drift and includes vehicle speed, full stationarity and standard deviation or DOP. The Azimuth and gyroscope drift update depend also on the first two position checks.

The navigation solution can operate in a hybrid mode that takes advantage of the benefits of both loosely and tightly coupled schemes. This mode benefits from the superior performance of some loosely coupled updates suitable for high-drift-rate low cost MEMS-based inertial sensors integrated with GPS (namely azimuth update from GPS when adequate and update of the gyroscope drift from GPS when suitable), as well as the benefits of tightly-coupled integration of using less than four satellites when available (a situation discarded by loosely coupled integration). When the availability and the quality of GPS position and velocity readings pass the assessment described above, a loosely-coupled measurement update is performed for position, velocity, azimuth, and gyroscope drift. Each update is performed according to its own quality assessment. Whenever the testing procedure detects degraded GPS performance, either because the visible satellite number falls below four or because the GPS quality examination failed, the filter switches to tightly-coupled update mode. Furthermore, the measurements from each satellite are assessed independently of those of the other satellites to check whether it is adequate to use as an

update. This check again exploits the higher performance of the Mixture PF for 3D RISS/GPS integration with higher order AR modeling of the gyroscope drift, since this solution can work unaided for elongated periods with only small degradation of performance. Thus the pseudorange estimate for each visible satellite to the receiver position estimated from the prediction phase of the Mixture PF is compared to the corresponding measured pseudorange to detect degradation in individual satellite measurements (for example those because of the presence of reflections with loss of direct line-of-sight). The satellites with degraded measurements are discarded, while other satellites are used for the update.

IV. MIXTURE PF FOR 3D RISS/GPS/MAP DATA

INTEGRATION

First the map matching technique is described, then its usage within Mixture PF is presented.

The map matching technique comprises several steps. The term solution will be used for a certain navigation state that will be the input to the map matching to get a new solution. At a certain time epoch, the predicted solution is used with the map data to find the candidate segments. There are four steps that are implemented to choose the candidate road segment(s). The first step is to narrow the search space to road segments that have one end within a certain distance (1000 m) of the solution position and thus decreases the overall processing time. The second step is to narrow the candidates further by checking the solution azimuth and keeping the road segments whose azimuth is within a certain angle (45º) from the solution azimuth, while taking care of both possible road directions in two-way roads. The third step is to check if the solution position is within the remaining candidate road segments with a margin of error (i.e. just outside the road segment). The road segments that do not fulfill this criterion are removed from the candidate pool. The reason for this step is to remove road segments whose projections may have a small perpendicular distance to the solution position but they are far off. The fourth step is to get the perpendicular distance between the solution position and the remaining candidate segments. The segment with minimum distance is chosen for this solution point, and the intersection of the perpendicular to the segment from this solution is the new position solution.

As discussed earlier PF gives more flexibility, so the map information is not just employed as a matching or pull-back step but it is tightly used inside the filter itself. Each particle in the sample set is constrained by the map information. The first two steps above for pruning the candidate segments are implemented on the mean of the sample set, or in other words, the solution that is used in these two steps is the mean of the sample set. This is because these are the two general pruning steps for the candidate road segments; this helps decreasing the computational load and the running time per iteration. However the next steps with the remaining candidate road segments are applied on each particle in the sample set, i.e.


each particle will be the said solution that is used in the remaining steps. This will lead to one choice of road segment per particle and thus one intersection point per particle. This helps in cases where multiple hypotheses need to be followed by the particle cloud and nothing can be discarded. This is also what achieves the tight integration of map data within the integration filter. When time advances, the correct hypothesis will quickly prevail and the others will be removed.

The proposed integration of the RISS/GPS solution with map matching can be considered a tight integration as opposed to loose integration which is adopted in the pull-back strategy and which is commonly used in most current commercial map-matching products.

V. EXPERIMENTAL RESULTS

The performance of the proposed Mixture PF with 3D RISS/GPS/Map data integration is examined with road test experiments in a land vehicle. The inertial sensors used inside RISS in the presented experiments are from a MEMS-grade IMU. The gyroscopes of this IMU have biases in the range of 2º/sec, the specifications are shown in Table I. The forward speed derived from the vehicle odometer is collected through the on board diagnostics interface using a data logging device. A relatively low-cost single frequency GPS receiver is used in these experiments for integration with RISS.

The results are evaluated with respect to a reference solution that utilize a high-end tactical grade IMU (With Ring Laser gyroscopes) integrated with a high-end dual frequency GPS receiver. This integrated solution provided the reference to validate the proposed method and to examine the performance during different conditions including degraded and completely blocked GPS.

The map data used to demonstrate and validate the proposed technique is from a 2005 street map database (but any street map in vector form can be used). The data are in shape files and include all types of road ways: detailed streets, highways, rural roads, and urban roads. The coordinates are the start points and end points of line segments of every road. A piecewise linear solution is used to represent curved roads.

Several road test trajectories were carried out using the setup described above. Two road test trajectories in downtown Toronto, Ontario, Canada are considered in this paper. In the first trajectory the integration of GPS uses a loosely coupled integration scheme, whereas in the second trajectory the integration of GPS uses a hybrid tightly/loosely coupled integration scheme. The map data integration in both trajectories is in a tight mode.

The first trajectory is shown in Figure 1. This road test was performed for nearly 128 minutes of continuous vehicle navigation and a distance of around 33.5 km was traveled. This trajectory is a downtown scenario with urban canyons in some parts and some underpasses. It had a lot of degraded GPS performance because of either severe signal reflection without a direct line of sight or complete blockage. The portions with degraded GPS performance encompass straight portions, turns, and frequent stops.

TABLE I USED MEMS-GRADE IMU SPECIFICATIONS

Specifications Gyroscopes Range ±100 º/s Bias < ±2.0 º/s Scale Factor < 1% Angle Random Walk

< 0.0375 º/s/ Hz

Specifications Accelerometers Range ± 19.6 m/s2

Bias < ± 0.294 m/s2 Scale Factor <1% Velocity Random Walk

< 0.0025 m/s2/ Hz Linearity < 1%

In this trajectory two solutions are presented: the Mixture

PF with 3D RISS/GPS/Map, and the Mixture PF with 3D RISS/GPS. The proposed Mixture PF with 3D RISS/GPS/Map had an RMS position error of 6.72m, and the Mixture PF with 3D RISS/GPS had an RMS position error of 10.73m.

Figure 2 and Figure 3 show the reference, GPS, and the two solutions during some sections of this trajectory. The GPS receiver suffered from severe multipath effects and from some complete blockage during these urban canyons as demonstrated in these figures. The Mixture PF with 3D RISS/GPS solution shows a very competitive performance for a navigation solution relying on such low-cost inertial sensors (with gyroscope biases of 2º/s) and with such severely degraded GPS. The Mixture PF with 3D RISS/GPS/Map solution demonstrates an extremely competitive performance considering the specifications and the very low cost of the sensors used.

The second trajectory can be seen in Figure 4. This road test was performed for nearly 158 minutes of continuous vehicle navigation and a distance of around 43.8 km was traveled. Like the first one, this trajectory had a lot of degraded GPS performance because of either reflections with loss of direct line-of-sight or complete blockage. The portions with degraded GPS performance encompass straight portions, turns, and frequent stops.

In this trajectory two solutions are presented: the Mixture PF with 3D RISS/GPS/Map, and the Mixture PF with 3D RISS/GPS. The proposed Mixture PF with 3D RISS/GPS/Map had an RMS position error of 6.22m, and the Mixture PF with 3D RISS/GPS had an RMS position error of 9.19m. Figure 5 shows the reference, the GPS, and the two solutions during some sections of this trajectory. These results confirm the results of the first trajectory and demonstrate the hybrid scheme.

The presented results for the Mixture PF are achieved with a number of samples equal to 100 with 20 samples predicted from observation likelihood. One iteration of the Mixture PF with 3D RISS/GPS takes approximately 0.0014 s (average of all iterations) on a laptop. Using the same laptop, one iteration of the Mixture PF with 3D RISS/GPS/Map takes approximately 0.0056 s (average of all iterations) when using the mean of the particles for all the map matching steps and takes approximately 0.0180 s (average of all iterations) when using all the particles starting from the third step as described


earlier. This shows that the algorithm can work in real-time, especially that these given run-times are for an interpreter-based language not a compiler-based language which can provide much faster run-time. The processor used was a 2.4 GHz processor, but slower processors with a compiler-based language can provide faster performance.

The reader is referred to [32] for real-time performance of the Mixture PF (without map matching) on an embedded 600 MHz processor (single core used), where a speed-up was proposed that enable Mixture PF to operate at higher rates on such embedded processors.

Figure 1: First trajectory in downtown Toronto.

Figure 2: A scenario in downtown Toronto from the first trajectory.

Figure 3: The portion with worst GPS in the first trajectory.

Figure 4: Second trajectory in downtown Toronto.

Figure 5: The downtown part of the second Toronto trajectory.

VI. CONCLUSION

This paper presented a low cost navigation device for land vehicles involving a reduced number of MEMS-based inertial sensors augmented with the measurements of the vehicle odometer and integrated with GPS and map data. The integration technique used was a Mixture PF that enables the utilization of nonlinear models, advanced modeling for the inertial sensors stochastic errors, and tight integration of the map data with this integrated navigation system.

The performance of the proposed integrated navigation solution was demonstrated to be very competitive for vehicle navigation with low-cost sensors. This solution can be used in all environments including degraded GPS environments which routinely occur in urban and rural canyons. The method has been verified on real-life road tests in downtown scenarios. The results have been examined to verify the suitability and satisfactory performance of the proposed solution even in downtown trajectories with degraded/multipath or totally denied GPS for long durations.

REFERENCES [1] Z.Yao, M. Lu, and Z. Feng, “ Spreading code phase measurement

technique for snapshot GNSS receiver,” IEEE Trans. Consum. Electron., vol. 56, no. 3, pp. 1275-1282, August 2010.

[2] J.-C. Juang, and Y-H Chen, “Accounting for data intermittency in a software GNSS receiver,” IEEE Trans. Consum. Electron., vol. 55, no. 2, pp. 327-333, May 2009.


[3] M. Sanchez-Fernandez, M. Aguilera-Forero, and A. Garcia-Armada, “Performance analysis and parameter optimization of DLL and MEDLL in fading multipath environments for next generation navigation receivers,” IEEE Trans. Consum. Electron., vol. 53, no. 4, pp. 1302-1308, November 2007.

[4] J. A. Farrell, Aided Navigation: GPS with High Rate Sensors, McGraw-Hill Professional, 2008.

[5] M. S. Grewal, L. R. Weill, and A. P. Andrews, Global Positioning Systems, Inertial Navigation, and Integration, 2nd ed., New Jersey: Wiley-Interscience, 2007.

[6] I. Skog and P. Handel, “In-car positioning and navigation - a survey”, IEEE Trans. Intell. Transp. Syst., vol. 10, no. 1, pp. 4-21, March 2009.

[7] R. Carlson, J. Gerdes, and J. Powell, “Error sources when land vehicle dead reckoning with differential wheelspeeds,” Navigation, vol. 51, no. 1, pp. 13–27, 2004.

[8] M. Quddus, W. Ochieng, and R. Noland, “Integrity of map-matching algorithms,” Transp. Res., Part C: Emerg. Technol., vol. 14, no. 4, pp. 283–302, August 2006.

[9] D. Obradovic, H. Lenz, and M. Schupfner, “Fusion of sensor data in siemens car navigation system,” IEEE Trans. Veh. Technol., vol. 56, no. 1, pp. 43–50, January 2007.

[10] C. Fouque, P. Bonnifait, D. Bétaille, “Enhancement of global vehicle localization using navigable road maps and dead-reckoning,” in Proc. of IEEE/ION PLANS 2008, Monterey, California, USA, May 2008, pp. 1286 - 1291.

[11] C. E. White, D. Bernstein, and A. L. Kornhauser, "Some map matching algorithms for personal navigation assistants," Transp. Res., Part C: Emerg. Technol., vol. 8C, no.1, pp. 91-108, 2000.

[12] S. Lee, D. Lee, and S. Lee, “Network-oriented road map generation for unknown roads using visual images and GPS-based location information,” IEEE Trans. Consum. Electron., vol. 55, no. 3, pp. 1233-1240, August 2009.

[13] Z. Ahmed, H. Jamal, R. Mehboob, and S. Khan, “A navigation device with MAC supporting multiple physical networks for extended coverage and operations,” IEEE Trans. Consum. Electron., vol. 54, no. 3, pp. 1103-1109, August 2008.

[14] J. Georgy, A. Noureldin, M. Korenberg, and M. Bayoumi, "Low cost three dimensional navigation solution for RISS/GPS integration using mixture particle filter," IEEE Trans. Veh. Technol., vol. 59, no. 2, pp. 599-615, February 2010.

[15] A. Noureldin, T.B. Karamat, M. D. Eberts, A. El-Shafie, “Performance enhancement of MEMS based INS/GPS integration for low cost navigation applications,” IEEE Trans. Veh. Technol., vol. 58, no. 3, pp. 1077-1096, March 2009.

[16] A. Doucet, S.J. Godsill, and C. Andrieu, “On sequential monte carlo sampling methods for bayesian filtering,” Stat. and Comput., vol. 10, pp. 197–208, November 2000.

[17] H.L. Dyckman, S. Sloat, and B. Pettus, “Particle filtering to improve GPS/INS integration”, in Proc. of the ION GNSS 2004, Long Beach, CA, USA , September 2004, pp. 1619-1626.

[18] Y. Yi and D.A. Grejner-Brzezinska, “Tightly-coupled GPS/INS integration using unscented kalman filter and particle filter”, in Proc. of ION GNSS 2006, Fort Worth, TX, USA, September 2006, vol. 4, pp. 2182-2191.

[19] A. Giremus, J.-Y. Tourneret, and P.M. Djuric, “An improved regularized particle filter for GPS/INS integration”, in Proc. 6th IEEE Int. Workshop SPAWC, New York, NY, USA, June 2005, pp. 1013-1017.

[20] A. Giremus, A. Doucet, V. Calmettes, and J.-Y. Tourneret, “A rao-blackwellized particle filter for INS/GPS integration”, in Proc. of IEEE Int. Conf. on Acoust., Speech, and Signal Process., 2004, vol. 3, pp. iii 964-967.

[21] F. Caron, M. Davy, E. Duflos, and P. Vanheeghe, “Particle filtering for multisensor data fusion with switching observation models: application to land vehicle positioning”, IEEE Trans. Signal Process., vol. 55, n. 6, pp. 2703-2719, June 2007.

[22] P. Aggarwal, D. Gu, S. Nassar , Z. Syed and N. El-Sheimy, “Extended particle filter (EPF) for INS/GPS land vehicle navigation applications”, in Proc. of ION GNSS 2007, Fort Worth, Texas, USA , September 2007, pp. 2619-2626.

[23] P. Aggarwal, Z. Syed, and N. El-Sheimy, "Hybrid extended particle filter (HEPF) for integrated inertial navigation and global positioning systems," Meas. Sci. and Technol., vol. 20, May 2009.

[24] U. Iqbal, A. F. Okou, and A. Noureldin, “An integrated reduced inertial sensor system - RISS/GPS for land vehicle”, in Proc. of IEEE/ION PLANS 2008, Monterey, California, USA, May 2008, pp.912-922.

[25] J. Georgy, U. Iqbal, M. Bayoumi, and A. Noureldin, “Reduced inertial sensor system (RISS)/GPS integration using particle filtering for land vehicles,” in Proc. of ION GNSS 2008, Savannah, Georgia, USA, September 2008, pp. 30-37.

[26] N.J. Gordon, D.J. Salmond, and A.F.M. Smith, “Novel approach to nonlinear/nongaussian bayesian state estimation,” in Proc. Inst. Elect. Eng. –F, Radar Signal Process., vol. 140, pp. 107–113, 1993.

[27] F. Dellaert, D. Fox, W. Burgard, and S. Thrun, “Monte carlo localization for mobile robots,” in Proc. of IEEE ICRA-99, Detroit, MI, USA, 1999, pp. 1322–1328.

[28] P. Jensfelt, “Approaches to mobile robot localization in indoor environments”, Ph.D. thesis, Department of Sensors, signals and systems, Royal Institute of Technology, Sweden, 2001.

[29] P. Jensfelt, O. Wijk, D. Austin, and M. Andersson, “Experiments on augmenting condensation for mobile robot localization,” in Proc. of IEEE ICRA-2000, vol. 3, pp. 2518–2524, San Francisco, CA, USA, 2000, pp. 2518–2524.

[30] S. Thrun, D. Fox, W. Burgard, and F. Dellaert, “Robust monte carlo localization for mobile robots,” Artif. Intell., vol. 128, pp. 99–141, 2001.

[31] M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial on particle filters for online nonlinear/non-gaussian bayesian tracking”, IEEE Trans. Signal Process., vol. 50, no. 2, pp. 174–188, February 2002.

[32] M.M. Atia, J. Georgy, M.J. Korenberg, A. Noureldin, “Real-time implementation of mixture particle filter for 3D RISS/GPS integrated navigation solution”, Electron. Lett., vol.46, no.15, July 2010.

BIOGRAPHIES

Jacques Georgy (S’09-M’10) received his Ph.D. degree in Electrical and Computer Engineering from Queen’s University, Canada in 2010. He received the B.Sc. and M.Sc. degrees in Computer and Systems Engineering from Ain Shams University, Egypt, in 2001 and 2007, respectively. He is working in positioning and navigation systems for vehicular, machinery and portable applications with Trusted

Positioning Inc., Calgary, AB, Canada. His research interests include linear and nonlinear state estimation, positioning and navigation systems, autonomous mobile robot navigation, and underwater target tracking.

Aboelmagd Noureldin (M’98-SM’08) received the B.Sc. degree in electrical engineering and the M.Sc. degree in engineering physics from Cairo University, Giza, Egypt, in 1993 and 1997, respectively, and the Ph.D. degree in electrical and computer engineering from The University of Calgary, Calgary, AB, Canada, in 2002. He is a Cross-Appointment Associate Professor with the Department of Electrical and Computer

Engineering, Queen’s University, Kingston, ON, Canada, and the Department of Electrical and Computer Engineering, Royal Military College (RMC) of Canada, Kingston. He is also the Founder and Leader of the Navigation and Instrumentation Research Group, RMC. His research is related to artificial intelligence, digital signal processing, spectral estimation and denoising, wavelet multiresolution analysis, and adaptive filtering, with emphasis on their applications in mobile multisensor system integration for navigation and positioning technologies. Dr. Noureldin is the Chair of the Alternative Integration Methods Research Group, International Association of Geodesy.

Chris Goodall has been working in developing, deploying and testing multi-sensor navigation systems for over 7 years. He obtained his B.Sc. in Systems Design Engineering at the University of Waterloo, Canada in 2004 and his PhD in Geomatics Engineering in 2008 from the University of Calgary. In 2009 Chris co-founded Trusted Positioning Inc., where he is the VP of Applications. Chris is responsible for the architecture

and design of these integrated navigation and positioning systems, used for applications such as vehicle navigation, machine control and mobile phone location.

Ieee Paper - 1

Documents

Transcript of Ieee Paper - 1