ION GNSS+ 2014, Session D6, Tampa, FL, 8-12 September 2014 Page 1/10

Development of a Tightly Coupled Vision/GNSS

System

Bernhard M. Aumayer, Mark G. Petovello, Gérard Lachapelle

University of Calgary

BIOGRAPHIES

Bernhard M. Aumayer is a PhD candidate in the Position,

Location and Navigation (PLAN) Group of the Department

of Geomatics Engineering at the University of Calgary. He

received his diploma degree in Hardware Software

Systems Engineering from the University of Applied

Sciences, Hagenberg, Austria in 2006. He worked for

several years as a software design engineer in GNSS

related R&D at u-blox AG, Switzerland. He expects to

complete his PhD in 2015.

Dr. Mark Petovello is a professor in the Position, Location

And Navigation (PLAN) Group in the Department of

Geomatics Engineering at the University of Calgary. He

has been actively involved in the navigation community

since 1997 and has received several awards for his work.

His current research focuses on software-based GNSS

receiver development and integration of GNSS with a

variety of other sensors.

Professor Gérard Lachapelle holds a Canada Research

Chair in Wireless Location in the PLAN Group. He has

been involved with GPS developments and applications

since 1980. His research ranges from precise positioning to

GNSS signal processing. More information is available on

the PLAN Group website

(http://plan.geomatics.ucalgary.ca).

ABSTRACT

This paper focuses on the loose and tight integration of a

stereo-vision system with a Global Navigation Satellite

System (GNSS) based navigation system. A simplified

vehicular dynamic model was chosen and implemented in

order to gain maximum advantage from the vision sensor’s

information. Data collected in open-sky as well as urban

environments was processed in post-mission with the

University of Calgary’s GSNRx™ software GNSS

receiver.

The results include performance measures of the loose and

tight integration approaches using high-quality GPS-

synchronized computer vision cameras, which are

compared to the output of a GNSS-only solution as well as

a tactical-grade reference trajectory.

More than 20,000 images and 30 minutes of raw GNSS

measurements in a dynamic scenario with a total travelled

distance of more than 10 km were collected, processed and

analyzed, and the resulting errors in the trajectories were

compared.

INTRODUCTION

Although early approaches of vision based navigation

reach back a few centuries [1], a large amount of research

in the area of vision-based navigation with significant

improvements in accuracies has been achieved and

published just recently [2, 3, 4], which shows the increased

attention of both research and industry in this technology.

Datasets recorded on vehicles with a series of optical

sensors have been created [5] for benchmarking computer

vision algorithms including visual odometry.

Very accurate motion estimation in the range of 1.6%

motion and 0.006 degrees heading errors per meter

travelled [2] was shown to be possible from the data

extracted solely from vision sensors, but at least an initial

accurate position and attitude is needed to allow continuous

estimation of earth-relative position and attitude. In post-

processing, a surveying approach or use of a reference

system is feasible, the integration with a GNSS receiver’s

position, velocity and time estimates has been suggested.

Although loose integration is suboptimal in terms of

performance, integration is widely performed in this

manner [6, 7, 8, 9], mostly for reasons of integration effort

and availability of raw range and range rate measurements

from GNSS receivers.

Tight integration in combination with inertial measurement

units (IMU’s) and monocular vision for ground vehicles

was shown recently [10, 11], as well as integration of

GNSS and vision for aerial applications [12].

In the case of loose integration the integration filter has a

reduced capability of identifying single range or range rate

measurement outliers, and is furthermore unable to use

GNSS measurements which are of insufficient number to

generate a GNSS position, velocity and time (PVT)

solution. Integrating the measurements in a tight manner,

i.e. using satellite pseudorange and pseudorange rate

measurements instead of the GNSS based PVT solutions,

eliminates these limitations and forms the main

ION GNSS+ 2014, Session D6, Tampa, FL, 8-12 September 2014 Page 2/10

contribution of this work. In contrast to already published

research, no IMU sensor was used, and an automotive-

specific dynamic model was implemented. This research

therefore focuses on improvements of integrating vision

measurements and GNSS measurements on the automotive

platform, rather than on improvements of the two

subsystems themselves.

The objectives of this work are to quantify the position

accuracy improvement in open-sky and urban canyon

scenarios by using a tightly integrated GNSS/vision

approach and comparing it to loosely coupled GNSS/vision

integration as well as traditional GNSS-only filter

estimates.

METHODOLOGY

SYSTEM DESIGN

The integrated GNSS/vision system was designed as a

tightly coupled Kalman filter combining the pseudorange

and range-rate measurements from the GNSS system with

the position- and attitude change rates from the vision

system. The loosely coupled filter was implemented for

comparison reasons and uses GNSS position and velocity

estimates from a least-squares solution rather than

pseudoranges and range-rates.

The system block diagram in Figure 1 shows the

combination of the ego-motion estimation from the vision

system with the measurements from the GNSS system for

the tightly coupled filter, making a GNSS-only based

position, velocity and time (PVT) solution as it is used in

the loose integration unnecessary.

Figure 1: System design block diagram

IMAGE RECTIFICATION

The cameras were calibrated using the method described in

[13, 14], using a chessboard calibration pattern mounted on

a planar wooden board. Several different views of the

pattern (Figure 2) were used to estimate the camera

calibration parameters to compute a rectified stereo image

pair with aligned image centers and horizontal epipolar

lines.

Figure 2: Stereo camera calibration sample image

EGO-MOTION ESTIMATION

The user’s ego-motion and orientation change can be

described by a translation and rotation of three axes and

transform the relative positions of the tracked feature

points from one time epoch to the next. The method used

in here is the same as used before and described in [15].

GNSS / VISION COMBINED ESTIMATION

Due to the non-linearity of the system, an extended Kalman

filter (EKF) is implemented, and instead of the original

states ( x ), error states (x ) are estimated based on the

most recent value of the original states.

A dynamic model for the automotive platform is used to

predict the new states at a high rate to constrain

linearization and small angle approximation errors, and the

measurements from GNSS and the vision system are both

integrated via measurement updates. The initial position,

speed and azimuth are initialized from an initial least-

squares solution, solving for the azimuth ( A ) using

arctan e

n

VA

V

(1)

where eV and nV denote the GNSS based velocity

estimates in east and north direction respectively, and from

error propagation, its corresponding variance can be

obtained by

2 22

2 2 2

2 2

3

2 2

1/ /

1 ( / ) 1 ( / )

/

(1 ( ) )/

e n

n e

n e n

V V

e n e n

e n

V V

e n

A

V V V

V V V V

V V

V V

(2)

where 2

( ) denotes the variance corresponding to the

covariance matrix element ( ) .

It is noted that due to the nonlinearity of the system and

small-angle approximations, this form of initialization

might cause a divergence of the filter, especially when the

vehicle is not moving at the time of initialization, as the

initial azimuth estimate can contain large errors in that

case. Nevertheless, no divergence of the integration filter

was experienced with the collected data.

The original state vector in equation (3) consists of the

local geodetic position, speed and acceleration, azimuth

ION GNSS+ 2014, Session D6, Tampa, FL, 8-12 September 2014 Page 3/10

and azimuth rate and the clock drift and corresponding

clock bias

(3)

with , and h denoting the geodetic latitude, longitude

and height respectively, s the speed, a the acceleration, A

and A the azimuth and azimuth rate of the system, cdt

the clock bias and .

cdt the clock drift. It is noted that with

this 2D navigation model, the roll and pitch of the vehicle

are neglected. The state vector for the loose integration

filter is similar but does not contain the two clock states.

The system model is defined as

( ) ( ) ( ) G(t) ( )d

t F t t tdt

x

x w (4)

using the dynamics model F calculated as

9 3

0 0

0 0

0 10

0

1

0 0

1

sin( )

cos( )

( ) ( )

0 0

x

A

M h

c

F

A

N h os

(5)

with M denoting the meridian radius of curvature and N

denoting the prime vertical radius of curvature at the

current latitude.

The noise shaping matrix which is defined as

0 0 0 0 0

0 0 0 0 0

1 0 0 0 0

0 0 0 0 0

0 1 0 0 0

0 0 0 0 0

0 0 1 0 0

0 0 0 1 0

0 0 0 0 1

G

(6)

maps the noise vector

. ][ T

h a cdtAcdt

w w w www (7)

to the states. It is noted that noise is only modeled for the

height, acceleration, azimuth rate and clock states, which

propagates through the dynamic model to the remaining

states.

The transition matrix is defined by equation (8).

2 32

,

3

1 ...2! 3!

k k

F t F tI F t

(8)

Following equation (3), the error state vector is defined as

containing the errors in the north ( N ), east ( E ) and up (

U ) direction, speed, acceleration, azimuth and azimuth

rate as well as errors in clock states where ( ) represents

the error in parameter ( ) .

The error states’ dynamic model is defined by equation (9)

where the subscript denotes the model’s correspondence

to the error states as opposed to the original states.

9 3

cos( ) 0 sin( ) 0 0

sin( ) 0 cos( )

0 0 0

0 1

0 0

1

0 0

1

0 0

x

A s A

A s A

F

(9)

The transition matrix for the error states can be computed

using equation (8) with F replaced with F .

Equations (10) and (11) are used for prediction and update

of the original and error states [16]. After each iteration,

the errors are applied to the original states using equation

(12).

1 , 1

ˆ ˆk k k k

x x (10)

ˆk k kK x z (11)

ˆ ˆ ˆk k k x x x (12)

The superscript minus and plus respectively denote the

state before and after an update. The Kalman gain is

determined by

1( )T T

k k k k k k kK P H H P H R (13)

where H denotes the design matrix which is the derivative

of the measurements with respect to the states.

The covariance of the error states after the prediction is

determined by

1 , 1 , 1

T

k k k k k k kP P Q

(14)

ION GNSS+ 2014, Session D6, Tampa, FL, 8-12 September 2014 Page 4/10

with kQ denoting the process noise matrix. The covariance

after a filter update is determined by

( )k k k kP I K H P (15)

Depending on the type of update (GNSS or vision

measurements), some states are not observable directly, but

rather via correlation with other states.

Position and velocity measurements obtained from a GNSS

least-squares estimation are integrated in the loosely

coupled filter via the measurement errors

,

ˆ( )(M )

ˆ( )(N )(cos )

ˆ

ˆˆcos( ))

ˆˆsin

(

( ( ))

G

G

G L G

n

e

h

h

V

V A

h h

s A

s

z (16)

where the subscript ‘G’ denotes the measurements from the

GNSS system, and the hat ̂ denotes the current estimate

of the state.

The covariance matrix of the measurements is used directly

from the estimated covariance matrix of the states of the

GNSS least-squares estimation filter, which used a signal

power relative weighting of the measurement covariance

[17].

For the tightly coupled filter, the measurements to the

Kalman filter are the errors in the range and range rate to

each satellite i, expanded at the current estimate of the

states

.

,

ˆ( )

ˆ( )

i i

G T i i

P c

P cdt

dt

z (17)

where iP and iP denote the pseudorange and pseudorange

rate to the satellite, ˆi and ˆ

i denote the estimated

geometric range and range rate, and cdt and .

cdt denote

the estimated clock bias and clock drift.

Similar to the GNSS least-squares solution measurement

weighting, a signal power relative weighting of the GNSS

measurements was applied.

The vision system measurement vector ( Vz ) is obtained

directly from the ego-motion estimation and has the form

ˆ

ˆV

zs

t

AA

t

z (18)

where z denotes the vision-estimated distance in forward

direction of the vehicle body, A denotes the vision-

estimated change in vehicle rotation around the vehicle

vertical axis, and t denotes the elapsed time between the

corresponding frames. Therefore, rather than observing

position and azimuth change, their rates are observed

which are similar to the speed and azimuth range states.

The measurement variances are obtained from the state

covariance of the vision system’s least-squares ego-motion

estimator, which assumes a Gaussian distributed

measurement error of the pixel location measurements.

It is noted that as long as the states are not initialized by

GNSS measurements, the vision system updates will not

provide enough information to estimate an absolute

position, velocity or attitude.

DATA INTEGRATION

As the azimuth rate in the collected data set exceeds 5

degrees in 100 ms, the prediction step was performed at a

higher rate of 100 Hz to reduce linearization and small-

angle approximation errors. Unlike the traditional

INS/GNSS integration approach where the INS data is used

in the prediction step, the measurements from the vision

system are used as updates to the filter in here. The separate

prediction with the dynamic model allows outlier detection

being performed not only on GNSS measurements, but also

on measurements from the vision subsystem. Furthermore,

as no measurements are needed for the prediction, the

estimation output rate is not constrained to the

measurement rate of the vision system, as opposed to the

traditional INS integration, where the highest output rate is

typically equal to the IMU measurement rate.

The vision system’s updates are processed at 10 Hz and are

synchronized (although the system would not require them

to be) with GPS time to the top of the GPS second. The

updates from the commercial grade GPS receiver are

constrained to the measurement rate of the receiver, and

performed at a 1 Hz rate in this work. The discrete

processing steps are visualized in Figure 3.

Figure 3: Prediction and update timeline

The flow diagram for data processing is shown in Figure 4.

‘PLAN-nav’ denotes the GNSS-only navigation solution

module of the University of Calgary’s GSNRx™ software

GNSS receiver, ‘PLAN-nav-vis’ denotes the integrated

GNSS/vision navigation solution module developed for

this research and the estimated outputs on the right side of

the figure are the GNSS-only least-squares and Kalman

filter solutions, as well as the loosely-coupled and tightly-

coupled integrated GNSS/vision solutions, which are

ION GNSS+ 2014, Session D6, Tampa, FL, 8-12 September 2014 Page 5/10

respectively denoted as ‘KF LC’ and ‘KF TC’. In this work

only GPS L1 C/A code signals were used due to

availability of measurements from the used hardware

receiver.

Figure 4: Hardware and software for processing data

OUTLIER DETECTION

A statistical outlier test is performed for both loose and

tight integration and for both GNSS and vision system

measurements, rejecting one outlier at a time and iterating

until either all remaining measurements are accepted or no

measurements remain for update. The confidence interval

for selecting the threshold was chosen as 99.9%.

DATA COLLECTION

HARDWARE

A stereo camera consisting of two Point Grey cameras

(Table 1) was used to collect data at the same time as a

consumer-grade u-blox 6T GPS receiver. The NovAtel

SPAN® LCI tactical-grade GNSS inertial system was used

as a reference. The data from the reference rover was

processed with NovAtel Inertial Explorer using tightly

coupled differential carrier phase processing with a base

station at the University of Calgary’s rooftop.

Figure 5: Stereo camera mounting arm and GNSS

antennas as well as tactical grade IMU on test vehicle

The cameras were mounted on a solid aluminum arm

which was mounted to the roof-rack of a test vehicle. The

image data was transported via Gigabit Ethernet to a data

recording laptop.

Figure 6: Point Grey BlackFly™ camera on the solid

aluminum mounting arm

The cameras use a GPS-time synchronized hardware

shutter pulse, which is synchronized to the calibrated GPS

receiver’s on-board main oscillator when no GPS position

can be obtained. A pulse is generated by the cameras when

an image is recorded, which is time-tagged by the GPS

receiver. The accuracy of the time tagging is typically on

the order of tens to hundreds of nanoseconds when a GPS

position can be obtained, and significantly better than

100 us for short outages, so in any case shorter than the

shortest exposure time (1 ms) of the camera, and therefore

deemed insignificant.

Table 1: Stereo vision system under test

Imager type,

Shutter

Brand /

Model Resolution

Base-

line

Data

transport

CCD,

global GPS-

synchronized

shutter

Point Grey

BlackFly™ 1280x720 100 cm

Gigabit

Ethernet

MEASUREMENTS

Data collections were performed on the campus of the

University of Calgary (‘Campus’) and in downtown

Calgary (‘Downtown’). The PLAN Group’s test vehicle

was used as a carrier for the system (Figure 5).

DATA ANALYSIS, EXPERIMENTAL RESULTS

Two dynamic tests were performed in Calgary, where in

one test the vehicle was static during initialization and in

the other it was moving during initialization. The GNSS

pseudorange and range rate measurements were taken from

the u-blox 6 GPS receiver and post-processed with the

PLAN-nav offline post-processing navigation software.

The measurements were processed in different

configurations:

‘LS GPS’ refers to a least-squares PVT solution using

GPS range and range rate measurements only

‘KF GPS’ refers to a GPS-only Kalman filter using

local level position and velocity states as widely

implemented in consumer grade receivers

‘KF GPS CAR’ refers to a GPS-only position

estimation using the automotive-specific dynamic

model as described in the previous section with speed

and azimuth states instead of the local level velocities.

ION GNSS+ 2014, Session D6, Tampa, FL, 8-12 September 2014 Page 6/10

The GNSS/vision integrated solution denoted with ‘KF

LC’ integrates measurements from the vision system with

the ‘LS GPS’ PVT solution in a loosely coupled manner.

Similarly, the ‘KF TC’ solution integrates vision

measurements and GNSS pseudo-ranges as well as range-

rates in a tightly coupled manner in a Kalman filter for

which the dynamic model was chosen similar to the ‘KF

GPS CAR’ filter. The only difference between the ‘KF TC’

and ‘KF GPS CAR’ is the usage of measurements from the

vision system. The power spectral densities of the random

walk stochastic processes were identical for the two filters

and in a similar range for the filters using different dynamic

models.

DYNAMIC TEST ‘CAMPUS’

In this test, the vehicle was initially stationary in a parking

lot, and after a few seconds started moving around the

campus of the University of Calgary (Figure 7). No

additional information (like orientation or initial position)

has been used from the reference system, information

solely from the GPS receiver and vision system was used

for the estimation in the integrated filters. The initial

heading therefore was very inaccurate, which is not

surprising due to unavailable absolute heading information

when the vehicle is not moving, which was also reflected

in the estimated variances. After initial movement, the

heading quickly converged towards the true heading.

Although the majority of the trajectory (western part of

trajectory shown in Figure 7) was under open-sky or light

foliage conditions, a short section towards the end in the

north-east part of the trajectory was affected by large

GNSS measurement errors, likely due to signal reflections

off adjacent high buildings (Figure 8), which is highlighted

in the analysis below.

Figure 7: Map of ‘Campus’ data collection trajectory.

Start point is at lower left corner of the map.

Figure 8: Challenging GNSS environment between ICT

(upper left), ES (lower left) and EEEL (right) buildings

near the north-east part of the trajectory on campus.

North is towards the upper right corner of this

perspective.

The statistics of the 2D position errors of the entire

trajectory are shown in Table 2. The estimated trajectory of

the tightly integrated filter shows a significantly lower

amount of large outliers compared to the GNSS-only least-

squares and loosely integrated filter estimates, and a

slightly better performance compared to the GNSS-only

Kalman filter implementations. Interestingly, due to slow

recovery from an outlier included in the solution, the

loosely-coupled integrated approach is outperformed by all

other solutions in terms of 2D RMS error.

Table 2: Statistics of ‘Campus’ full trajectory.

Filter 2D RMS

error [m]

2D max

error [m]

LS GPS 7.2 47.1

KF GPS 2.2 7.9

KF GPS CAR 2.5 7.0

KF LC 7.4 24.4

KF TC 1.8 5.2

In the following, a selection of sections with significant

differences in the estimates of the different filters are

analyzed in detail in order to illustrate the benefits and

drawbacks of including vision data in the system.

Figure 9 shows the reference and estimated trajectories at

the roundabout in the north-east of the trajectory, which

can be seen at the upper left corner of the perspective view

in Figure 8. Especially where large outliers in the GNSS

measurements are present (south-west part in Figure 9),

which were undetected in the least-squares GNSS position

estimation, the tight integration shows advantages of

improved outlier detection capabilities compared to all

other solutions.

ION GNSS+ 2014, Session D6, Tampa, FL, 8-12 September 2014 Page 7/10

Figure 9: Outliers in GNSS measurements affecting

GNSS-only and loosely integrated estimates

At the 180 degree turn on the east side of the EEEL

building at the north end of the campus (central building in

Figure 8), outliers in both the GNSS and the vision system

happened and introduced major position errors in the GPS-

only least-squares and loosely coupled estimates (Figure

10), but could be detected and rejected from the solution

by the tightly coupled and GPS-only Kalman filters. While

the source of the GNSS measurement error is unknown it

is likely a multipath effect. The outlier in the vision

measurement happened due to several wrong feature

correspondences which were not detected due to a reduced

number of features while the vehicle was turning, and

therefore the vision system had to acquire new features

while recently tracked features went out of view.

Figure 10: Loosely integrated solution showing largest

position estimate error due to subsequent undetected

outliers in both GNSS and vision subsystems

Figure 10 also shows the least-squares GPS estimate which

was used as the input to the loosely coupled integration

filter. Single outliers exceeding 40 m can be observed in

the least-squares estimated trajectory, which also affect the

loosely coupled integrated estimate.

DYNAMIC TEST ‘DOWNTOWN’

Figure 11 shows the trajectory through downtown Calgary.

At the start of this dataset in the south-west, the vehicle is

already in motion with a speed of approximately 14 m/s

along a road with open-sky visibility. As the vehicle is

entering the urban area from the south west, the north side

gets obstructed with high buildings, and after another short

open-sky section at the east part of the trajectory the

vehicle enters an urban canyon which is exited at the north

west end after a 30-minute drive through the city’s

downtown core.

Figure 11: Trajectory through Calgary downtown

The error statistics for the entire trajectory are shown in

Table 3, where it is noteworthy that the RMS error for the

integrated solutions is larger than the RMS error for the

GPS-only solutions. The reason for these results are

undetected outliers which lead to overly optimistic position

and attitude accuracy estimates while experiencing

significant errors, which—combined with the chosen

dynamic model—propagate forward several epochs and

prevent the solution from recovering.

Table 3: Statistics of ‘Downtown’ full trajectory.

Filter 2D RMS

error [m]

2D max

error [m]

LS GPS 50.6 472.2

KF GPS 29.7 149.3

KF GPS CAR 36.2 159.0

KF LC 57.6 376.7

KF TC 34.6 160.5

In the following, again a selection of short sections is

analyzed which show a significant divergence of the

estimated solutions.

The first GNSS-challenging section with a single side

obstructed sky view (buildings on north side of trajectory),

as shown in Figure 12, highlights the importance of

rejecting GNSS range measurement outliers. While the

least-squares solution is not able to detect the large errors

in the measurements, resulting in position errors larger than

150 m, the traditional approach with a Kalman filter which

implements position and velocity states is less affected by

ION GNSS+ 2014, Session D6, Tampa, FL, 8-12 September 2014 Page 8/10

those outliers. The loosely integrated solution, in turn, is

significantly affected as it lacks the strength to reject single

satellite’s measurements from the least-squares GNSS

solution. In contrast, the tight integration approach showed

smallest errors in the 2D position in this section.

Figure 12: First GNSS challenging section of downtown

data set

In deep urban canyons, the continuous tracking and outlier

rejection capability of the vision system’s motion

estimations significantly contributes to the integrated

position estimation accuracy. For example, Figure 13

shows part of the trajectory involving two turns around a

single city block. Several GNSS measurements are

significantly affected by measurement errors, and the

position estimation error exceeds 100 m for the GNSS-only

least-squares solution and 50 m for the GNSS-only

Kalman-filter solution. While the loosely coupled filter

estimate is significantly affected even in sections where the

GNSS-only Kalman filter solution is close to the reference,

the tightly coupled solution shows the smallest maximum

error in this section.

Figure 13: Trajectory around a building block in an

urban canyon downtown Calgary

The statistics of this section of the trajectory are shown in

Table 4. While the least-squares solution expectedly shows

worst position accuracy, in this section the standard

Kalman filter solution estimates experience significantly

larger errors than the estimates from the Kalman filter

which is using the automotive-specific model. The tightly

integrated solution performs best in terms of 2D RMS

error, although it experiences a larger maximum error than

the equivalent solution without vision information.

Table 4: Statistics of ‘Downtown’ section shown in

Figure 13.

Filter 2D RMS

error [m]

2D max

error [m]

LS GPS 50.2 191.5

KF GPS 31.2 67.3

KF GPS CAR 25.2 40.9

KF LC 37.7 142.9

KF TC 23.2 57.1

Comparing the GPS-only least-squares position solution to

the integrated solutions and the GPS-only Kalman filter

estimate emphasizes the existence of undetected outliers in

the GNSS measurements, and the importance to detect

them in order to obtain an accurate position solution in a

challenging environment. With the additional information

provided by the vision sensor, the capability of the

integrated filter to detect those outliers improves.

At the end of the trajectory (Figure 14) where the vehicle

was reverse parked into a parallel parking stall in a sub-

urban area, the GNSS-only solution was affected by errors

in the measurements, which led to a slowly drifting

position towards the east with a maximum position error of

about 30 m. However, the tightly integrated solution could

constrain the RMS error to less than 11 m.

Figure 14: Endpoint of trajectory ‘Downtown’ in sub-

urban area

Although there were several moving objects in view while

the vehicle was stationary, they could be identified reliably

as outliers in this section, and the tightly integrated solution

performs best in terms of 2D RMS and maximum position

error as shown in the statistics in Table 5.

ION GNSS+ 2014, Session D6, Tampa, FL, 8-12 September 2014 Page 9/10

Table 5: Statistics of section at end of ‘Downtown’

trajectory as shown in Figure 14.

Filter 2D RMS

error [m]

2D max

error [m]

LS GPS 37.0 83.5

KF GPS 23.9 32.9

KF GPS CAR 11.3 16.2

KF LC 26.4 30.3

KF TC 10.7 13.6

PROCESSING SPEED

The image processing was performed on an Intel®

Core™ i5 CPU @ 2.40 GHz where image rectification,

corner detection, feature extraction and feature tracking on

the 1280x720 pixels of each of the two images could be

performed within an average of 200 ms per frame.

Performing these standard computer vision algorithms on

the graphics processing unit (GPU) showed significant

performance improvements for the processing task itself,

but was slowed down by data transfer overhead which is

not optimized in the current implementation. Ego-motion

estimation using approximately 100 tracked feature points

took between 10 and 1000 ms, depending on the number of

iterations resulting from outlier rejection. One second of

predictions as well as GNSS and vision updates in the

integration Kalman filter used about 30 ms of CPU time.

CONCLUSIONS

A tight form of integrating measurements from a stereo

vision system with GNSS measurements was presented

and the results from a series of different filtering

approaches using GNSS and combined GNSS/vision

measurements was compared using data collected in an

open-sky and urban canyon area. The position solutions

from GPS-only least-squares and Kalman estimation filters

were compared to estimates from GPS-only estimates

using an automotive-specific dynamic model within a

Kalman filter, as well as to an integrated GNSS/vision

solution in a loosely and tightly coupled manner.

The type of estimator (least-squares versus Kalman filter),

the configuration of sensors used (GNSS-only versus

GNSS/vision) and chosen dynamic model (local level

position and velocities versus automotive-specific model)

were shown to significantly influence the accuracies of the

estimates in the tested environments. While a large amount

of outliers in the GNSS measurements as well as vision

measurements stayed undetected in the loosely coupled

approach, a significant amount of those outliers could be

identified and rejected from the solution using the vehicle

specific dynamic model in combination with vision

updates in the tightly coupled filter. Although the tight

integration approach showed superior performance relative

to the other approaches for the whole trajectory of the

open-sky test and at key points of the urban canyon

trajectory, the overall performance of the tight integration

in the urban canyon test was inferior to the universal

GNSS-only Kalman filter implementation. The main

reason for this is the combination of undetected outliers

and the used dynamic model which does not allow fast

recovery from errors in the state estimates.

As the measurements in the vision system implemented in

this work contain a significant amount of outliers, and the

motion estimates from the vision system herein seem

significantly less reliable than from other recent

publications, additional effort will be necessary to detect

outliers more reliably from the vision subsystem.

Furthermore, an ultra-tight integration with the GNSS

system is pursued in future research, which is expected to

improve the accuracies of the GNSS measurements and

therefore the accuracies of also the estimated states.

ACKNOWLEDGEMENTS

We thank Alberta Innovates - Technology Futures (AITF)

for financial support. u-blox is acknowledged for providing

u-blox 6 GNSS receivers, and thanks is extended to

NVIDIA for providing a CARMA embedded computing

platform.

REFERENCES

[1] E. D. Dickmanns, A. Zapp, and K. D. Otto, “Ein

Simulationskreis zur Entwicklung einer automatischen

Fahrzeugführung mit bildhaften und inertialen Signalen,”

in Simulationstechnik, 1984, pp. 554–558.

[2] H. Badino, A. Yamamoto, and T. Kanade, “Visual

odometry by multi-frame feature integration,” in Computer

Vision Workshops (ICCVW), 2013 IEEE International

Conference on, Dec 2013, pp. 222–229.

[3] W. Lu, Z. Xiang, and J. Liu, “High-performance

visual odometry with two-stage local binocular BA and

GPU,” in Intelligent Vehicles Symposium (IV), 2013 IEEE,

June 2013, pp. 1107–1112.

[4] A. Comport, E. Malis, and P. Rives, “Accurate

quadrifocal tracking for robust 3d visual odometry,” in

Robotics and Automation, 2007 IEEE International

Conference on, April 2007, pp. 40–45.

[5] A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision

meets robotics: The KITTI dataset,” International Journal

of Robotics Research (IJRR), 2013.

[6] J. Rehder, K. Gupta, S. Nuske, and S. Singh, “Global

pose estimation with limited GPS and long range visual

odometry,” in Robotics and Automation (ICRA), 2012

IEEE International Conference on, May 2012, pp. 627–

633.

ION GNSS+ 2014, Session D6, Tampa, FL, 8-12 September 2014 Page 10/10

[7] D. Dusha and L. Mejias, “Error analysis and attitude

observability of a monocular GPS/visual odometry

integrated navigation filter,” The International Journal of

Robotics Research, vol. 31, no. 6, pp. 714–737, 2012.

[8] D. Schleicher, L. M. Bergasa, M. Ocaña, R. Barea,

and M. E. López, “Real-time hierarchical outdoor SLAM

based on stereovision and GPS fusion,” Intelligent

Transportation Systems, IEEE Transactions on, vol. 10,

no. 3, pp. 440–452, 2009.

[9] M. Pollefeys, D. Nistér, J.-M. Frahm, A. Akbarzadeh,

P. Mordohai, B. Clipp, C. Engels, D. Gallup, S.-J. Kim,

P. Merrell, C. Salmi, S. Sinha, B. Talton, L. Wang,

Q. Yang, H. Stewénius, R. Yang, G. Welch, and H. Towles,

“Detailed real-time urban 3D reconstruction from video,”

International Journal of Computer Vision, vol. 78, no. 2-3,

pp. 143–167, 2008.

[10] T. Chu, N. Guo, S. Backén, and D. Akos, “Monocular

camera/IMU/GNSS integration for ground vehicle

navigation in challenging GNSS environments,” Sensors,

vol. 12, no. 3, pp. 3162–3185, 2012.

[11] D. H. Won, E. Lee, M. Heo, S. Sung, J. Lee, and Y. J.

Lee, “GNSS integration with vision-based navigation for

low GNSS visibility conditions,” GPS solutions, vol. 18,

no. 2, pp. 177–187, 2014.

[12] P. J. Roberts, R. A. Walker, and P. J. O’Shea,

“Tightly coupled GNSS and vision navigation for

unmanned air vehicle applications,” 2005.

[13] R. Hartley, “Theory and practice of projective

rectification,” International Journal of Computer Vision,

vol. 35, no. 2, pp. 115–127, 1999.

[14] Z. Zhang, “A flexible new technique for camera

calibration,” Pattern Analysis and Machine Intelligence,

IEEE Transactions on, vol. 22, no. 11, pp. 1330–1334, Nov

2000.

[15] B. M. Aumayer, M. G. Petovello, and G. Lachapelle,

“Stereo-vision aided GNSS for automotive navigation in

challenging environments,” in Proceedings of the 26th

International Technical Meeting of The Satellite Division

of the Institute of Navigation (ION GNSS+ 2013),

Nashville, TN, September 2013, pp. 511–520.

[16] R. Brown and P. Hwang, Introduction to random

signals and applied Kalman filtering, 3rd ed. John Wiley

& Sons, Inc., 1997.

[17] B. Aminian, “Investigation of GPS observations for

indoor GPS/INS integration,” Master’s thesis, University

of Calgary, 2011.