sensors

Article
A Tightly-Coupled GPS/INS/UWB Cooperative
Positioning Sensors System Supported by
V2I Communication
Jian Wang 1 , Yang Gao 2,3 , Zengke Li 1 , Xiaolin Meng 2,3, * and Craig M. Hancock 4
1 School of Environmental Science and Spatial Informatics, China University of Mining and Technology,
Xuzhou 221116, China; wjianhuance@163.com (J.W.); zengkeli@cumt.edu.cn (Z.L.)
2 Nottingham Geospatial Institute, The University of Nottingham, Nottingham NG7 2TU, UK;
gis_gaoyang@163.com
3 Sino-UK Geospatial Engineering Centre, The University of Nottingham, Nottingham NG7 2TU, UK
4 Department of Civil Engineering, University of Nottingham, Ningbo 315100, China;
craig.hancock@nottingham.edu.cn
* Correspondence: xiaolin.meng@nottingham.ac.uk; Tel.: +44-115-846-6029

Received: 22 March 2016; Accepted: 8 June 2016; Published: 27 June 2016

Abstract: This paper investigates a tightly-coupled Global Position System (GPS)/Ultra-Wideband

(UWB)/Inertial Navigation System (INS) cooperative positioning scheme using a Robust Kalman
Filter (RKF) supported by V2I communication. The scheme proposes a method that uses range
measurements of UWB units transmitted among the terminals as augmentation inputs of the
observations. The UWB range inputs are used to reform the GPS observation equations that consist
of pseudo-range and Doppler measurements and the updated observation equation is processed in a
tightly-coupled GPS/UWB/INS integrated positioning equation using an adaptive Robust Kalman
Filter. The result of the trial conducted on the roof of the Nottingham Geospatial Institute (NGI) at the
University of Nottingham shows that the integrated solution provides better accuracy and improves
the availability of the system in GPS denied environments. RKF can eliminate the effects of gross
errors. Additionally, the internal and external reliabilities of the system are enhanced when the UWB
observables received from the moving terminals are involved in the positioning algorithm.

Keywords: cooperative positioning; GPS; INS; Robust Kalman Filter; UWB; V2I

1. Introduction
Cooperative positioning is one of the key implementation techniques for the application of
Intelligent Transportation Systems (ITS) and therefore this has become an area of research interest in
recent years. Cooperative positioning, which is also called collaborative positioning, is a positioning
approach in which a number of participants contribute their position-related information. To explain
the advantage of cooperative positioning, the whole is greater than the sum of its parts may be the
best way to interpret its benefits of sharing information. In a cooperative positioning system, two or
more participants are allowed to work cooperatively to improve their position performance by sharing
relevant location information [1].
When a vehicle tries to obtain its own position, GPS is undoubtedly the first choice to be applied
extensively due to its global coverage and ease of use. However, GPS often suffers from signal
interference, multipath, and blockage, especially when a vehicle is travelling in an urban canyon.
To compensate for the shortcomings of the GPS signal, extra observations transmitted from other
transportation participants and sensors are desired. Aiming to improve traffic safety and enhance
transportation efficiency, the U.S. Federal Communication Commission allocated 75 MHz of Dedicated
Short Range Communication (DSRC) spectrum between 5.85 GHz and 5.925 GHz to be used exclusively

Sensors 2016, 16, 944; doi:10.3390/s16070944 www.mdpi.com/journal/sensors

Sensors 2016, 16, 944 2 of 16

for Vehicle to Vehicle (V2V) and Vehicle to Infrastructure (V2I) communication (referred to as V2X
communication). Bridging the communication gaps among transportation participants, they are able
to cooperatively position their locations by talking with surrounding participants for exchanging
information via V2X communication. The authors of [2] suggest a cooperative vehicle positioning
method by exchanging GPS coordinates and range measurements and the simulated results reveal
that a vehicle could recognise about 70% of the surrounding vehicle with an error less than 1 m. In
reference [3], researchers demonstrated a peer-to-peer cooperative localisation method by combining
GNSS coordinates and terrestrial ranging measurements and the simulated 2D accuracy could achieve
decimetre-level accuracy under three aiding peers cases. The authors in [4] proposed a cooperative
positioning algorithm to improve localisation accuracy of vehicles by 15% using inter-vehicle distance
and angle measurements in the Vehicular Ad hoc Network (VANET). A distributed location estimate
algorithm has also been proposed to improve the positioning accuracy by extracting inter-vehicle
distance from GPS pseudo-range measurements [5].
To further enhance the reliability and availability of a cooperative positioning system, many
researches on multi-sensor integration and the corresponding algorithms have been performed.
U.S. Federal Communications Commission established different technical standards and operating
restrictions for Ultra-wideband (UWB) that UWB devices must operate in the frequency band
3.110.6 Hz within the measurement systems domain. Due to UWBs large bandwidth, it has excellent
multipath resolving capability, which is critical for positioning application. UWB has the capacity
of penetration as well as immunity of multipath. The pseudo-range and Doppler of a Differential
GPS (DGPS) and the observations of a UWB system are used as the inputs into a federated filter to
perform decentralized cooperative computations for vehicles positions. A VANET is designed and the
UWB inter-vehicle ranges were used to compensate the systematic errors [6]. In reference [7], UWB
ranging was applied to augment the DGPS to improve relative positioning precision. However, all
the measurements gathered from all the vehicles were used in the V2V positioning architecture. Thus
its robustness and flexibility are limited due to the limitation in the communication connectivity. In
reference [8], the author fused neighbours GPS position and odometer-based speed via DSRC and own
GPS observation into an Extended Kalman Filter (EKF) to enhance position accuracy. It shows this
method can improve the accuracy up to 48% compared with GPS positioning alone depending on the
speed of the participating vehicles. Researchers in [9] invented a Pseudo-VRS solution to share RTK
correction via inter-vehicle communication and the result shows that the proposed solution can help
vehicle received RTK correction maintaining the availability of ambiguity fixed solutions is at least
97% as long as transmission latency is lower than 20 s. The UWB range observations are also used
to augment GPS to resolve relative position between vehicles and the accuracy of baseline between
vehicles could achieve better than 0.3 m in partially obstructed urban canyon [6,7]. A tightly-coupled
GPS and UWB system is comprehensively investigated in [1012]. The results show that the proposed
system improves positioning accuracy and availability of fixed ambiguity solution especially in a GPS
hostile environment.
Moreover, GPS and UWB cannot be expected to achieve 100% coverage sometimes due to their
natures of line-of-sight technology, especially in urban canyon and tunnels. Inertial Navigation System
(INS) has the advantage of self-contained navigation capacity that can be integrated with GPS and
UWB to improve the availability and reliability of a vehicle-positioning unit. In the case that the feed
of GPS and UWB observations is less than four, INS could still help to estimate the vehicles position.
In reference [13], a loosely-coupled integration of low-cost GPS/INS and UWB solution is invented to
provide about 0.2 m accuracy when both GPS and UWB observations are obtainable.
The purpose of this paper is to investigate the performance of a tightly-coupled GPS/UWB/INS
cooperative positioning scheme supported by V2X communication. This paper is mainly focused on
the V2V application; however, the approaches described within this paper are also applicable to V2I
scenarios. The remainder of this paper is organized as follows. First of all, the application scenario of
proposed scheme where a V2I communication network has been distributed is depicted in Section 2.
Sensors 2016, 16, 944 3 of 16

Sensors 2016, 16, 944 3 of 16

Section 3 demonstrates the technical structure of proposed cooperative positioning scheme. Then, the
Robust
GPS, INSKalman
and UWBFiltermeasurements
(RKF) is presented
at thethrough
Section 4.enumerating the observation
Finally, experimental equation
tests and involving
discussions are
GPS, INS and
described UWB 5measurements
in Section followed by some at theconclusions
Section 4. inFinally, experimental
Section 6. tests and discussions are
described in Section 5 followed by some conclusions in Section 6.
2. Cooperative Positioning for V2X Application
2. Cooperative Positioning for V2X Application
The V2X communication architecture as a realisation of the cooperative positioning has been
The
developed V2Xfor communication
several years. It architecture
is assumed that as aarealisation
Vehicular of
Adthe
hoc cooperative positioning
Network (VANET) has been
is established
developed for several years. It is assumed that a Vehicular Ad hoc Network (VANET)
based on V2X communication, as shown in Figure 1. The protocol that demonstrates how a vehicle is established
based onits
resolves V2X communication,
position through the as shown
proposed in Figure
scheme1.areThepresented
protocol that demonstrates
as follows: Firstly,how
eacha vehicle
vehicle
resolves its position through the proposed scheme are presented as follows: Firstly,
positioning unit calculates its own position using available data collected from both equipped sensors each vehicle
positioning unit calculates
and surrounding vehicles viaits VANET,
own position using available
with respect data collected
to the current from both equipped
time. Simultaneously, the unit
sensors and surrounding vehicles via VANET, with respect to the current
transmits its own measurements and the calculated position to the neighbouring vehicles. time. Simultaneously,
the unit transmitseach
Consequentially, its own measurements
vehicle positions andand the calculated
sends position towhen
data recurrently the neighbouring
they are runningvehicles.
on
Consequentially,
the road. each vehicle positions and sends data recurrently when they are running on the road.

Figure 1. The V2X application in an urban canyon.

Figure 1. The V2X application in an urban canyon.

The V2X concept is illustrated that vehicles driven in an urban canyon are assumed to receive
The V2X concept
measurements from notis illustrated that vehicles
only neighbouring driven
vehicles butinalso
an urban canyon are
infrastructure assumed
points. Thesetodata
receive
are
measurements from not only neighbouring vehicles but also infrastructure points. These
then used to calculate the vehicles positions. By taking advantages of V2X communication, vehicles data are then
used to calculate
are able to sharetheand
vehicles positions.
exchange By taking
information advantages
with surrounding of V2X communication,
transportation vehicles are
participants for
able
important safety applications, such as lane change assistance, intersection collision important
to share and exchange information with surrounding transportation participants for warning,
safety applications,
overtaking such as lane
vehicle warning, rearchange assistance,
end collision intersection
warning, collision warning, overtaking vehicle
etc. [14].
warning, rear end collision warning, etc. [14].
However, a vehicle cannot always resolve its position when only GPS and UWB measurements
However, from
are obtained a vehicle cannot
above V2Xalways resolve its position
communication when only
architecture. The GPS and UWB
reasons couldmeasurements
be the poor
are obtained from above V2X communication architecture. The reasons could
performance of the GPS/UWB geometry, the limited operational range of the UWB radios be the poor performance
and a
of the GPS/UWB geometry, the limited operational range of the UWB radios
complete blockage of GPS signals, etc. To further enhance the availability and reliabilityand a complete blockage
of the
of GPS signals,
cooperative etc. To further
positioning system, enhance the availability
an Inertial Navigation and reliability
System of theto
(INS) needs cooperative
be embedded positioning
into the
system, an Inertial Navigation System (INS) needs to be embedded into the GPS
GPS and UWB integration positioning system and implementing algorithms need to be developed. and UWB integration
positioning system and implementing algorithms need to be developed.
3. Tightly-Coupled GPS/UWB/INS Cooperative Positioning
3. Tightly-Coupled GPS/UWB/INS Cooperative Positioning
The schematic of the cooperative positioning scheme supported by V2I communication using
The schematic of the cooperative positioning scheme supported by V2I communication using
GPS, UWB and INS measurements is given in Figure 2. For each vehicle positioning unit, the raw INS
GPS, UWB and INS measurements is given in Figure 2. For each vehicle positioning unit, the raw
measurements of accelerometer and gyroscope are fed into and Robust Kalman Filter (RKF) together
INS measurements of accelerometer and gyroscope are fed into and Robust Kalman Filter (RKF)
with UWB range measurements and the GPS coordinates of the infrastructure points and the nearby
together with UWB range measurements and the GPS coordinates of the infrastructure points and the
vehicles to solve the coordinate. A DSRC module is used to trade its estimated coordinate for
neighbours coordinates and ranges with surrounding transportation participants at the beginning
and end of a positioning cycle.
Sensors 2016, 16, 944 4 of 16

nearby vehicles to solve the coordinate. A DSRC module is used to trade its estimated coordinate for
neighbours coordinates and ranges with surrounding transportation participants at the beginning
and end
Sensors of16,
2016, a positioning
944 cycle. 4 of 16

Figure
Figure 2.
2. The
The schematic
schematic of
of proposed
proposed cooperative
cooperative positioning method.
positioning method.

The RKF is applied to fuse all measurements of the multi-sensors, including pseudo-range,
The RKF is applied to fuse all measurements of the multi-sensors, including pseudo-range,
Doppler measurement, UWB range, acceleration and angular rate, in order to enhance the reliability
Doppler measurement, UWB range, acceleration and angular rate, in order to enhance the reliability
of positioning capability. INS feeds its calculated pseudo-range and Doppler measurement into RFK
of positioning capability. INS feeds its calculated pseudo-range and Doppler measurement into
whilst GPS and UWB also contribute their pseudo-range, Doppler measurement and range
RFK whilst GPS and UWB also contribute their pseudo-range, Doppler measurement and range
observations. Then, the accelerometer bias and gyro drift generated from RKF are fed back to the INS
observations. Then, the accelerometer bias and gyro drift generated from RKF are fed back to the INS
for next epoch calculation when position, velocity and attitude are produced. It should be mentioned
for next epoch calculation when position, velocity and attitude are produced. It should be mentioned
that only position output is of interest here and velocity and attitude are not discussed in this paper.
that only position output is of interest here and velocity and attitude are not discussed in this paper.
4.
4. Robust
RobustKalman
KalmanFilter
Filterfor
forCooperative
CooperativePositioning
Positioning

4.1.
4.1. Dynamic
Dynamic Equation
Equation
The
The system
system error of the
error of the dynamics
dynamics model
model of
of integrated
integrated navigation
navigation used
used in the Kalman
in the Kalman Filter
Filter is
is
designed
designed based on the INS error equations. The insignificant terms are neglected in the process of
based on the INS error equations. The insignificant terms are neglected in the process of
linearization [15]. The
linearization [15]. The psi-angle
psi-angle error equations of
error equations of INS
INS are
are as
as follows
follows [16]:
[16]:

. r en r v (1)
r en r ` v (1)
v (2ie en ) v f
. (2)
v p2ie ` en q v f ` (2)
. (ie en )
pie ` en q ` (3)
(3)

where rv
where r, v and
, and are theare the position,
position, velocityvelocity and orientation
and orientation errorrespectively.
error vectors, vectors, respectively. en
en is the rate
is
ofthe rate of navigation
navigation frame with frame withtorespect
respect earth,to earth,
and is therate
ie and ie
is of
theearth
rate of earth
with with respect
respect to inertial
to inertial frame.
frame. f is the force
Besides,Besides, observation.
f is the Then, the
force observation. system
Then, theerror dynamics
system of GPS/INS
error dynamics integration
of GPS/INS is obtained
integration is
by expanding
obtained the accelerometer
by expanding bias errorbias
the accelerometer vector and
error vector and
the gyro drift
theerror
gyrovector . vector .
drift error
The accelerometer
The accelerometerbias biaserror
errorvector
vector and the gyro
and drift drift
the gyro errorerror
vectorvector
are regarded as the random
are regarded as the
walk process vectors, which are modelled as follows [17]:
random walk process vectors, which are modelled as follows [17]:
.
uu
(4)
. u
u (5)
andu u
where uuand are
are white
white Gaussian
Gaussian noise
noise vectors.
vectors.
The receiver clock state dynamic equations can be written as follows:
dt u
dt (6)
dt

u
dt dt (7)
Sensors 2016, 16, 944 5 of 16

The receiver clock state dynamic equations can be written as follows:

.
dt dt ` udt (6)
.
dt udt (7)

where udt and udt are white Gaussian noise vectors of receiver clock error and receiver clock error drift.
By combining Equations (1) and (7), the system dynamics model can be generalized in matrix and
vector form [18]: .
X FX ` u (8)

where X is the error state vector which contains position error, velocity error, attitude error,
accelerometer bias, gyro drift, clock error and clock drift. F is the system transition matrix, and
u is the process noise vector.

4.2. Observation Equation

The observation model in GPS/INS/UWB tightly-coupled positioning scheme is composed of
the pseudo-range and Doppler difference vector among the UWB, GPS observation and the INS
computation value [19]:
GPS
PjINS
fi
Pj
GPS
D j DINS
ffi
j ffi
Z UWB
INS
ffi (9)
ri ri ffi
fl
..
.

where PjGPS and DGPS

j are the pseudo-range and Doppler value observed by the jth GPS satellite,
respectively; PjINS and DINS
j are the pseudo-range and Doppler measurement of the jth satellite
predicted by INS, respectively; riINS is the space distance calculated via the three-dimensional
coordinates of a moving vehicle estimated using a strap down inertial system algorithm and the
three-dimensional coordinates of the UWB reference stations and riUWB is the UWB range measurements
of the ith UWB unit [20]:
b
2 ` 2 ` 2
riUWB
`
xUWB x ` yUWB y ` zUWB z (10)

where xUWB , yUWB and zUWB are the three-dimensional coordinates of the UWB reference stations and
x, y, z are the three-dimensional coordinates of a moving vehicle to be solved.
The generic measurement equation system of the Kalman Filter can be written as:

Zk H k X k ` (11)

where H k is the observation matrix [20], and is the measurement noise vector with covariance matrix
Rk , assumed to be white Gaussian noise.

4.3. Robust Kalman Filter

The optimal estimates of the state vector from the Kalman Filter can be reached through a time
update and an observation update. The time update process of Kalman Filter is independent, which is
written as:
X k Fk,k1 Xk1 (12)
T
C k Fk,k1 Ck1 Fk,k 1 ` Qk1 (13)
Sensors 2016, 16, 944 6 of 16

The observation update equation of Kalman Filter is expressed as:

Vk Zk Hk X k (14)

Xk X k ` Gk Vk (15)
1
T T 1
Gk C k Hk Hk C k Hk ` Rk (16)
k
Ck pI Gk Hk q C k (17)

where X k is a priori state estimation, Xk is a posteriori state estimation, Gk is the gain matrix of Kalman
Filter, C k is a priori covariance matrix of state vector, Ck1 is a posteriori covariance matrix of state
vector, Rk is the covariance matrix of measurement noise vector, Qk1 is the covariance matrix of
process noise, and Fk,k1 is the system transition matrix from epoch k 1 to epoch k, k is the robust
factor. The robust factor k is calculated by the IGGIII scheme to improve calculation efficiency through
a piecewise function. A different robust factor is set according to the value of the gross error [21,22]:
$

1, sk k0
k 1 s k 2
&
k0 (18)
k sk k 1 k 0 , k0 sk k1

% 1030 , s k

k 1

where k0 and k1 are constants which have the values k0 = 2.5~3.5 and k1 = 3.5~4.5, respectively.
a
sk Vk { k dk (19)

where Vk and dk are the predicted residual and their reciprocal of the weight of the observation vector,
k the variance of the unit weight can be written as:
a
k 1.4826 MEDI AN |Vk | { dk (20)

This section may be divided by subheadings. It should provide a concise and precise description
of the experimental results, their interpretation, as well as the experimental conclusions that can
be drawn.

5. Experimental Tests
Field tests were conducted to evaluate the performance of the proposed scheme on the roof
of the Nottingham Geospatial Institute (NGI) [23]. The test consists of one tactical grade Inertial
Measurement Unit (IMU), 3 UWB units, and 4 GPS receivers. The sampling rate of the UWB unit was
1 Hz and its ranging error will be evaluated in following section using a stochastic analysis model. The
specification of the IMU is given in Table 1.

Table 1. Tactical grade IMU technical specifications.

Parameters Gyroscope Accelerometer

Bias 1 /h 1.0 mg
Scale factor 150 ppm 300 ppm
Random walk 0.125 /sqrt(h) 6 mg/sqrt(Hz)
Sampling Rate 200 Hz

In the beginning, a Leica AS10 GNSS dual-frequency antenna was installed on the top of a pillar
above the NGI locomotive and a UWB unit was fastened under the antenna with a known lever-arm.
This UWB unit was also connected to a laptop to store the range observations between it and other
Sensors 2016, 16, 944 7 of 16

two moving UWB units. The SPAN IMU inside the locomotive was connected to the Leica antenna
and recorded raw observations into a SD card for post-processing. Then two people walked along
the track
Sensors 2016,and each of them was equipped with a set of equipment that included a GPS receiver,
16, 944 7 of 16a
UWB unit and a battery. The product type of the UWB unit is Lock-on Model LD2 provided by Thales
Thales Research
Research & Technology
& Technology Ltd.,Sussex,
Ltd., West West Sussex,
UK. TheUK. The
unit unit complies
complies with the with the standard
standard UWB power UWB
power density limit of 41.3 dBm/MHz, which leads to a nominal output
density limit of 41.3 dBm/MHz, which leads to a nominal output power of 100 W. The transmission power of 100 W. The
transmission
is is Frequency
Frequency Hopped DirectHopped
Sequence Direct
SpreadSequence
Spectrum Spread
signalSpectrum
covering:signal
4760 MHzcovering: 4760
to 6200 MHz
MHz. Theto
6200 MHz. The range accuracy of decimetre level can be achieved by UWB.
range accuracy of decimetre level can be achieved by UWB. To calibrate the GPS and the UWB antenna To calibrate the GPS and
the UWB the
precisely, antenna precisely,
GPS pillar thewith
fastened GPSUWB pillarwas
fastened with UWB
held vertically was held
as stable vertically
as possible as stable
during as
the tests.
possible GPS
Another during the tests.
receiver wasAnother
set on oneGPS receiver
of the pillarswas set NGI
on the on oneroofofto
the
actpillars
as theon the NGIstation.
reference roof toAs actthe
as
the reference
UWB station.
ranges were timeAstagged
the UWB ranges
by the were
laptop time tagged
system time, the bylaptop
the laptop system time, the
was synchronized laptop
to GPS timewasto
synchronized
collect 1 Hz UWB to GPS time
ranges to collect
prior to the 1test.
HzBefore
UWB ranges
startingprior to the
the tests, wetest. Before
assume thatstarting the tests,and
the locomotive we
assume
two thatwere
people the carrying
locomotive and two
a DSRC devicepeople
within were carrying acommunication
the reachable DSRC device within the reachable
range. The sampling
communication range. The sampling rate of GPS receivers and IMU were respectively
rate of GPS receivers and IMU were respectively configured as 10 Hz and 200 Hz to simulate a running configured as
10 Hz under
vehicle and 200 V2IHz to simulate aenvironment.
communication running vehicle under V2Iran
The locomotive communication
along the trackenvironment. The
to act as a vehicle
locomotive ran along the track to act as a vehicle that is able to exchange
that is able to exchange raw observations and distributed positioning coordinates with surrounding raw observations and
distributed
traffic positioning
participants coordinates
(thee two people). with surrounding
Therefore, traffic consists
a V2I network participants (thee two
of a running people).
vehicle, two
Therefore, a and
participants V2I anetwork
reference consists
stationofis aplotted
running in vehicle,
Figure 3atwoandparticipants
the devicesand useda during
reference thestation
tests are is
plotted in Figure
presented in Figure 3b.3a and the devices used during the tests are presented in Figure 3b.

(a)

(b)
Figure 3.
Figure 3. The
The test
testscenario
scenariofor
forverifying
verifyingthe
theproposed
proposed architecture.
architecture. (a)(a) Test
Test deployment
deployment diagram;
diagram; (b)
(b) the
the locomotive and GPS and the
locomotive and GPS and the UWB unit. UWB unit.

A GPS software tool called GrafNavTM (Version 8.0) is used to post-process the ambiguity-fixed
A GPS software tool called GrafNavTM (Version 8.0) is used to post-process the ambiguity-fixed
GPS RTK solution to obtain the trajectory of locomotive for verification purpose and the achievable
GPS RTK solution to obtain the trajectory of locomotive for verification purpose and the achievable
accuracies with post-processing are listed in Table 2. The DOP value variation during the
experimental period is illustrated in Figure 4. In the case that more than 4 visible GPS satellites shown
in Figure 4a, the PDOP value of the GPS only case is greater than 1 and the maximum value
approaches 2, while the PDOP value of the cooperative positioning by including UWB range is
approximately 1 throughout the experiment. When the number of the GPS satellites drops to 4, the
Sensors 2016, 16, 944 8 of 16

accuracies with post-processing are listed in Table 2. The DOP value variation during the experimental
period is illustrated in Figure 4. In the case that more than 4 visible GPS satellites shown in Figure 4a,
the PDOP value of the GPS only case is greater than 1 and the maximum value approaches 2, while the
PDOP2016,
Sensors value16,of
944the cooperative positioning by including UWB range is approximately 1 throughout 8 ofthe
16
experiment. When the number of the GPS satellites drops to 4, the PDOP value almost approaches to 20
remains
while theless
PDOPthanvalue
5 as of
shown in Figure 4b.
the cooperative It is illustrated
positioning that the
still remains fusion
less than of the
5 as GPS/UWB/INS
shown in Figure 4b.raw It
observations obtained
is illustrated that via of
the fusion V2I
thecommunication
GPS/UWB/INS network can greatly
raw observations improve
obtained thecommunication
via V2I reliability of
positioning
network cancapability.
greatly improve the reliability of positioning capability.

PDOP PDOP
2 20
GPS/INS/UWB GPS/INS/UWB
GPS/INS GPS/INS

PDOP
PDOP

1 10

0 0
486100 486400 486700 487000 486100 486400 486700 487000
time(s) time(s)

(a) (b)
Figure
Figure 4.
4. The
The DOP
DOP value
value variations
variations during
during the
the experimental
experimental period.
period. (a)
(a) In
In the
the case
case of
of more
more than
than four
four
GPS satellite; (b) in the case of four GPS satellite.
GPS satellite; (b) in the case of four GPS satellite.

Table
Table 2.
2. GPS
GPS accuracy
accuracy with
with post-processing.
post-processing.
Scenarios Horizontal Vertical
Scenarios
static 5 mm + 0.5 ppm 10 mm + 0.5Vertical
Horizontal
ppm
static
kinematic 10 5mm
mm++10.5
ppmppm 20 mm10
+ 1mm + 0.5 ppm
ppm
kinematic 10 mm + 1 ppm 20 mm + 1 ppm
To overall evaluate the performance of the cooperative positioning scheme, six scenarios are
considered. Theevaluate
To overall average velocity of the moving
the performance of theplatform frompositioning
cooperative all experience is about
scheme, six2 scenarios
m/s: are
considered. The average velocity of the moving platform from all experience is about 2 m/s:
Scenario 1: Integration of GPS/INS using pseudo-ranges, Doppler observations and raw INS
observations
Scenario for of
1: Integration cooperative
GPS/INS positioning in the caseDoppler
using pseudo-ranges, of more observations
than four satellites.
and raw INS
Scenario 2: Integration of GPS/UWB/INS using pseudo-ranges, Doppler observations, UWB range
observations for cooperative positioning in the case of more than four satellites.
and raw INS observations for cooperative positioning in the case of more than four
Scenario 2: Integration of GPS/UWB/INS using pseudo-ranges, Doppler observations, UWB
satellites.
range and raw INS observations for cooperative positioning in the case of more than
Scenario 3: Integration of GPS/INS using pseudo-ranges, Doppler observations and raw INS
four satellites.
observations for cooperative positioning in the case of only four satellites.
Scenario
Scenario 3: Integration
4: Integration of GPS/INS using
of GPS/UWB/INS usingpseudo-ranges,
pseudo-ranges,Doppler
Dopplerobservations
observations, and rawranges
UWB INS
observations for cooperative positioning in the case of only four satellites.
and raw INS observations for cooperative positioning in the case of only four satellites.
Scenario 4: Integration
Scenario 5: Integration of GPS/UWB/INS
of GPS/UWB/INS using using pseudo-ranges,
pseudo-ranges, Doppler
Doppler observations,
observations, UWBUWBranges
and rawranges
INSand raw INS observations
observations for cooperative
for cooperative positioning positioning in three,
in the case of the case of and
two onlyone
visiblefour satellites.
satellites.
Scenario 6: Integration
Scenario of GPS/UWB/INS
5: Integration of GPS/UWB/INS using using
pseudo-ranges, Doppler
pseudo-ranges, observations,
Doppler UWBUWB
observations, ranges
and raw INSand
ranges observations in the case of
raw INS observations forgross errors. positioning in the case of three, two
cooperative
and one visible satellites.
5.1. Stochastic Model of UWB Ranges
Scenario 6: Integration of GPS/UWB/INS using pseudo-ranges, Doppler observations, UWB
To obtain the stochastic
ranges parameters
and raw of the UWB
INS observations measurements,
in the autocorrelation function and
case of gross errors.
probability density function are analysed in an off-line manner. The residuals of a UWB data series
of 718 epochs with a frequency of 1 Hz are obtained via subtracting UWB range with the real distance
that are measured with a total station. The frequency analysis, shown in Figure 5, demonstrates little
information of the UWB raw measurements. The unbiased estimate of the autocorrelation coefficient
is calculated using autocorrelation function method, and also the probability density function of the
residuals is estimated. As shown in Figure 5a, the delay of the autocorrelation estimate of the
Sensors 2016, 16, 944 9 of 16

5.1. Stochastic Model of UWB Ranges

To obtain the stochastic parameters of the UWB measurements, autocorrelation function and
probability density function are analysed in an off-line manner. The residuals of a UWB data series of
718 epochs with a frequency of 1 Hz are obtained via subtracting UWB range with the real distance
that are measured with a total station. The frequency analysis, shown in Figure 5, demonstrates little
information of the UWB raw measurements. The unbiased estimate of the autocorrelation coefficient
is calculated using autocorrelation function method, and also the probability density function of the
residuals is16,
Sensors 2016, estimated.
944 As shown in Figure 5a, the delay of the autocorrelation estimate of the predicted
9 of 16
Sensors 2016, 16, 944 9 of 16
residuals is zero and the variance of the residual is 0.14 m2 . In Figure 5b, the absolute values of the
of the residuals
residuals of the UWB measurements
of the UWB are all less than 0.2 m thatthe
proves the relationship among
of the residuals of themeasurements are all less
UWB measurements are than 0.2than
all less m that
0.2proves relationship
m that proves among the
the relationship data
among
thelow.
is data is low.
the data is low.
70 1
70 1
60
60
50 0.5

Residuals(m)
50 0.5
Residuals(m)
Amplitude

40
Amplitude

40
0
30 0
30
UWB

20
UWB

20 -0.5
10 -0.5
10
0 -1
00 0.2 0.4 0.6 0.8 1 -1 0 200 400 600 800
0 0.2 0.4 0.6
Frequency(Hz) 0.8 1 0 200 400 600 800
time(s)
Frequency(Hz) time(s)
(a) (b)
(a) (b)
Figure 5. The
The residuals
residuals of
of the UWB
UWB measurement and and its frequency
frequency analysis result.
result. (a)
(a) Frequency
Frequency
Figure
Figure 5.
5. The residuals of the
the UWB measurement
measurement and its its frequency analysis
analysis result. (a) Frequency
characteristics
characteristics of the UWB signal; (b) the residuals of the UWB measurement.
characteristics of
of the
the UWB
UWB signal;
signal; (b)
(b) the
the residuals
residuals of
of the
the UWB
UWB measurement.
measurement.

As shown in Figure 6, the predicted residuals approximately coincide with the Gaussian
As shown in Figure 6, the predicted residuals approximately coincide with the Gaussian
distribution and the variance of 0.14 m22 2is then used as the value of the stochastic parameter in the
distribution and the variance of 0.14 m is then
and the variance of 0.14 m is then used as as
used thethe
value of the
value stochastic
of the parameter
stochastic in the
parameter in
mentioned Robust Kalman Filter (RKF) model.
mentioned
the Robust
mentioned Kalman
Robust Filter
Kalman (RKF)
Filter model.
(RKF) model.
0.2 1
(unbiased)

0.2 1
(coeff)
(unbiased)

(coeff)

0.1
0.1 0.5
Autocorrelation

0.5
Autocorrelation
Autocorrelation

0
Autocorrelation

0
0
-0.1 0
-0.1
-0.2 -0.5
-0.2
-1000 -500 0 500 1000 -0.5
-1000 -500 0 500 1000
-1000 -500Delay 0time/s500 1000 -1000 -500Delay 0time/s500 1000
Delay time/s Delay time/s
1
1
density
Probabilitydensity

0.5
0.5
Probability

0
-1.5
0 -1 -0.5 0 0.5 1 1.5
-1.5 -1 -0.5 0
Residuals/m 0.5 1 1.5
Residuals/m
Figure 6. Auto-correlation function and probability density function of residuals.
Figure 6. Auto-correlation
Auto-correlation function and probability density function of residuals.
According to Equation (8), the stochastic models of the parameters including position, velocity
According to Equation (8), the stochastic models of the parameters including position, velocity
and orientation, accelerometer bias and gyro drift are influenced by accelerometer and gyro
and orientation, accelerometer bias and gyro drift are influenced by accelerometer and gyro
observation. Thus, the IMU, which was fixed in a three-axis rotary table, was tested and the
observation. Thus, the IMU, which was fixed in a three-axis rotary table, was tested and the
observation was analysed using the autocorrelation analysis to obtain the spectral density. There was
observation was analysed using the autocorrelation analysis to obtain the spectral density. There was
difference of the spectral density for the accelerometer and gyro observation between in the lab
difference of the spectral density for the accelerometer and gyro observation between in the lab
environment and in the field test, so the value of stochastic model was set larger than the test value
environment and in the field test, so the value of stochastic model was set larger than the test value
according to experience. In the data processing, the parameters of the stochastic model of the dynamic
Sensors 2016, 16, 944 10 of 16

According to Equation (8), the stochastic models of the parameters including position, velocity and
orientation, accelerometer bias and gyro drift are influenced by accelerometer and gyro observation.
Thus, the IMU, which was fixed in a three-axis rotary table, was tested and the observation was
analysed using the autocorrelation analysis to obtain the spectral density. There was difference of the
spectral density for the accelerometer and gyro observation between in the lab environment and in the
field test, so the value of stochastic model was set larger than the test value according to experience. In
the data processing, the parameters of the stochastic model of the dynamic model were determined
based on experience. The initial position errors were 1.5 m, 1.5 m, and 3 m and the initial velocity errors
were 0.2 m/s, 0.2 m/s, and 0.8 m/s in NED directions, respectively. The initial platform alignment
errors were 0.1 , 0.1 , and 1 . According to the measurement accuracy of IMU, the initial standard
deviations of gyro and accelerometer biases were 15 /h and 600 mg, respectively.
Sensors 2016, 16, 944 10 of 16
Sensors 2016, 16, 944 10 of 16
5.2. GPS/INS/UWB Cooperative Positioning
5.2. GPS/INS/UWB Cooperative Positioning
5.2. seen
As GPS/INS/UWB
in Figure Cooperative Positioning
7, the trajectory of GPS/INS integration has a systematic bias mainly caused by
As seen in Figure 7, the trajectory of GPS/INS integration has a systematic bias mainly caused
the errors in errors
As
by the the
seenpseudo-range
ininFigure 7, theand Doppler
trajectory
the pseudo-range of observations.
GPS/INS
and Doppler The The
integration
observations. reasons for for
hasreasons thisthis
a systematic could
bias bebethe
mainly
could thesatellite
satelliteobit
caused
errors, clock
byobit errors
the errors,
errors atmospheric
inclock
the pseudo-range errors,
and etc. Figure
Doppler 8 and Table
observations. The 3 show
reasons that
for the
this positioning
could be
errors atmospheric errors, etc. Figure 8 and Table 3 show that the positioning the accuracies
satellite
obit errors,
(RMS) accuracies clock
in the north,
(RMS) errors
eastinand atmospheric
downeast
the north, areand errors,
improved etc.
down are Figure
76%, 8 and
61%, and
improved Table
76%,16%, 3 show that
61%,respectively. the positioning
and 16%, respectively.
accuracies (RMS) in the north, east and down are improved 76%, 61%, and 16%, respectively.
trajectory
52.9521 trajectory
52.9521 GPS/INS/UWB
GPS/INS/UWB
GPS/INS
GPS/INS
REFERENCE
REFERENCE
latitude/()
latitude/()

52.952
52.952

52.9519
-1.1841
52.9519 -1.1839 -1.1837 -1.1835
-1.1841 -1.1839 longitude/()
-1.1837 -1.1835
longitude/()
Figure 7. The trajectories of the locomotive in Scenario 1 and 2.
Figure 7. The trajectories of the locomotive in Scenario 1 and 2.
Figure 7. The trajectories of the locomotive in Scenario 1 and 2.
position error GPS/INS/UWB
4 position error GPS/INS
GPS/INS/UWB
42
north/m

GPS/INS
20
north/m

0-2
-2-4
486100 486400 486700 487000
-4
486100
2 486400 486700 487000
21
east/m

10
east/m

0-1
-1-2
486100 486400 486700 487000
-2
486100
2 486400 486700 487000
21
down/m

10
down/m

0-1
-1-2
486100 486400 486700 487000
-2
486100 486400 time/s 486700 487000
time/s
Figure 8. The position accuracies in Scenario 1 and 2.
Figure 8. The position accuracies in Scenario 1 and 2.
Figure 8. The position accuracies in Scenario 1 and 2.
Table 3. Comparison of Scenario 1 and 2 in terms of position error.
Table 3. Comparison of Scenario 1 and 2 in terms of position error.
RMS (m) MAX (m)
Scenario RMS (m) Down North MAX (m) Down
Scenario North East East
1 North
1.82 East
0.57 Down
0.68 North
3.30 East
1.60 Down
1.89
12 1.82
0.44 0.57
0.22 0.68
0.57 3.30
1.86 1.60
0.70 1.89
1.53
2 0.44 0.22 0.57 1.86 0.70 1.53
Sensors 2016, 16, 944 11 of 16

Table 3. Comparison of Scenario 1 and 2 in terms of position error.

RMS (m) MAX (m)

Scenario
North East Down North East Down
1 1.82 0.57 0.68 3.30 1.60 1.89
2 0.44 0.22 0.57 1.86 0.70 1.53

In the case of Scenario 3 and 4, i.e., only four visible GPS satellites, the comparison between
those two scenarios is shown in Figure 9. It is similar to Figure 7 in that the systematic error has been
removed
Sensors 2016,by
16,integrating
944 UWB observations. By integrating UWB range measurements, the cooperative 11 of 16
positioning accuracies in north, east and down are improved by 73%, 76%, and 33%, respectively,
in Figure
of only 4 10 and Table
satellites are 4.superior
It is alsotonoticed that
the case ofthe improvements
more of accuracy
than 4 satellites in the casethe
after integrating of only
UWB4
satellites
observation.are superior to the case of more than 4 satellites after integrating the UWB observation.

trajectory
52.9521
GPS/INS/UWB
GPS/INS
REFERENCE
latitude/()
latitude/()

52.952

52.9519
-1.1841 -1.1839 -1.1837 -1.1835
longitude/()

The trajectories
Figure 9. The trajectories of the locomotive in Scenario 3 and 4.

position error GPS/INS/UWB

4 GPS/INS
north/m
north/m

-4
486100 486400 486700 487000
4
east/m
east/m

-4
486100 486400 486700 487000
4
down/m
down/m

-4
486100 486400 486700 487000
time/s

Figure
Figure 10.
10. The
The position
position accuracies
accuracies in
in Scenario
Scenario 33 and
and 4.
4.

Table 4. Comparison between Scenario 3 and 4 in terms of position error.

RMS (m) MAX (m)

Scenario
North East Down North East Down
3 1.75 1.09 1.64 3.29 4.59 3.37
4 0.48 0.26 1.10 1.86 0.87 2.15
Sensors 2016, 16, 944 12 of 16

Table 4. Comparison between Scenario 3 and 4 in terms of position error.

RMS (m) MAX (m)

Scenario
North East Down North East Down
3 1.75 1.09 1.64 3.29 4.59 3.37
4 0.48 0.26 1.10 1.86 0.87 2.15

5.3. GPS/INS/UWB Cooperative Positioning in GPS Denied Environments

In Scenario 5, to verify the performance of the cooperative positioning scheme, three gaps of
GPS blockage are manually created. Three satellites are reserved between 486,400 s and 486,450 s,
two satellites are reserved between 486,600 s and 486,650 s, and just one satellite is reserved between
486,800 s and 486,850 s. The trajectories of three gaps are illustrated in Figure 11 and the position error
is demonstrated
Sensors 2016, 16, 944 in Figure 12. In the case of less than 4 satellites, the positioning errors enlarge 12with
of 16
Sensors 2016, 16, 944 12 of 16
time and the INS errors cannot be estimated correctly. The integration of UWB range measurement
can drag the trajectory
measurement can drag back to the right
the trajectory way
back toand
the remain relative
right way high accuracy.
and remain relativeAs listed
high in TableAs5,
accuracy.
measurement can drag the trajectory back to the right way and remain relative high accuracy. As
GPS/INS/UWB solution gives half-meter accuracies in the north and east directions
listed in Table 5, GPS/INS/UWB solution gives half-meter accuracies in the north and east directions during three
listed in Table 5, GPS/INS/UWB solution gives half-meter accuracies in the north and east directions
periodsthree
during whileperiods
the accuracies of GPS/INS
while the accuraciessolution are beyond
of GPS/INS oneare
solution meter. As the
beyond onelocomotive
meter. As wasthe
during three periods while the accuracies of GPS/INS solution are beyond one meter. As the
running on a flat track, the accuracy of height does not drift away a lot because of the
locomotive was running on a flat track, the accuracy of height does not drift away a lot because of loss of the GPS
locomotive was running on a flat track, the accuracy of height does not drift away a lot because of
satellite.
the loss ofIntheaddition, the accuracies
GPS satellite. in the the
In addition, north, east andinheight
accuracies directions
the north, gradually
east and heightdegrade on
directions
the loss of the GPS satellite. In addition, the accuracies in the north, east and height directions
account ofdegrade
gradually decreasing visible GPS
on account satellite. visible GPS satellite.
of decreasing
gradually degrade on account of decreasing visible GPS satellite.
trajectory trajectory trajectory
52.9521 trajectory 52.9521 trajectory 52.9521 trajectory
52.9521 GPS/INS/UWB 52.9521 GPS/INS/UWB 52.9521 GPS/INS/UWB
GPS/INS/UWB
GPS/INS GPS/INS/UWB
GPS/INS GPS/INS/UWB
GPS/INS
GPS/INS
REFERENCE GPS/INS
REFERENCE GPS/INS
REFERENCE
REFERENCE REFERENCE REFERENCE
latitude/()
latitude/()

latitude/()
latitude/()
latitude/()

latitude/()

52.952 52.952 52.952

52.952 52.952 52.952

52.9519 52.9519 52.9519

-1.1841
52.9519 -1.1839 -1.1837 -1.1835 -1.1841
52.9519 -1.1839 -1.1837 -1.1835 -1.1841
52.9519 -1.1839 -1.1837 -1.1835
-1.1841 -1.1839 longitude/()-1.1837 -1.1835 -1.1841 -1.1839 longitude/()-1.1837 -1.1835 -1.1841 -1.1839 longitude/()-1.1837 -1.1835
longitude/() longitude/() longitude/()

(a) (b) (c)

(a) (b) (c)
Figure 11.
Figure 11. The trajectories
Thetrajectories
11. The of
trajectoriesof the
ofthe locomotive
the locomotive in
locomotivein three
inthree GPS
three GPS blockage
GPS blockage periods
blockageperiods (Scenario
periods(Scenario 5).
(Scenario5). (a)
5).(a) one
(a)one gap
onegap
gap
Figure
with
with three
three satellites;
satellites; (b)
(b) one
one gap
gap with
with two
two satellites;
satellites; (c)
(c) one
one gap
gap with
with one
one satellite.
satellite.
with three satellites; (b) one gap with two satellites; (c) one gap
position error
4 position error
4 GPS/INS/UWB GPS/INS
2 GPS/INS/UWB GPS/INS
north/m

2
north/m

0
0
-2
-2
-4
486100
-4 486400 486700 487000
486100
8 486400 486700 487000
8
4
east/m

4
east/m

0
0
-4
-4
-8
486100
-8 486400 486700 487000
486100
4 486400 486700 487000
4
2
down/m

2
down/m

0
0
-2
-2
-4
486100
-4 486400 486700 487000
486100 486400 486700 487000
time/s
time/s

Figure 12.
Figure12. The position
Theposition
12.The accuracies
positionaccuracies in
accuraciesin three
inthree GPS
threeGPS blockage
GPSblockage periods
blockageperiods (Scenario
periods(Scenario 5).
(Scenario 5).
5).
Figure

Table 5. Position errors in three insufficient GPS satellites periods.

Table 5. Position errors in three insufficient GPS satellites periods.
GPS/INS/UWB (m) GPS/INS (m)
Gaps GPS/INS/UWB (m) GPS/INS (m)
Gaps North East Down North East Down
North East Down North East Down
486400s486450s 0.42 0.17 0.39 0.56 1.34 0.39
486400s486450s 0.42 0.17 0.39 0.56 1.34 0.39
486600s486650s 0.41 0.18 0.53 1.03 2.65 0.54
Sensors 2016, 16, 944 13 of 16

Table 5. Position errors in three insufficient GPS satellites periods.

GPS/INS/UWB (m) GPS/INS (m)

Gaps
North East Down North East Down
486400s486450s 0.42 0.17 0.39 0.56 1.34 0.39
486600s486650s 0.41 0.18 0.53 1.03 2.65 0.54
486800s486850s 0.46 0.17 0.58 0.54 0.92 0.58

5.4. Robustness Test of Robust Kalman Filter

To demonstrate the robustness of the model, simulated gross errors were added to the GPS and
UWB observations at the times of 486,420 s, 486,640 s and 486,820 s under Scenario 6. The trajectories
shown in Figure 13 demonstrate the effect of a gross error of 20 m added onto SV 27 pseudo-range
measurement and Figure 14 reveals the corresponding positioning error. The good shapes of green
trajectories prove that the effect of added gross error is eliminated by using RKF. As listed in Table 6,
RKF improves accuracies of 70%, 62%, and 19%, at epoch 486,820 s in the north, east and height
directions,
Sensors respectively,
2016, 16, 944 comparing against the accuracies of standard Kalman Filter. 13 of 16
Sensors 2016, 16, 944 13 of 16
trajectory trajectory trajectory
52.9521 trajectory 52.9521 trajectory 52.9521 trajectory
52.9521 KALMAN FILTER 52.9521 KALMAN FILTER 52.9521 KALMAN FILTER
KALMAN FILTER FILTER
ROBUST KALMAN KALMAN FILTER FILTER
ROBUST KALMAN KALMAN FILTER FILTER
ROBUST KALMAN
ROBUST KALMAN FILTER
REFERENCE ROBUST KALMAN FILTER
REFERENCE ROBUST KALMAN FILTER
REFERENCE
REFERENCE REFERENCE REFERENCE
latitude/()

latitude/()

latitude/()
latitude/()

latitude/()

latitude/()
52.952 52.952 52.952
52.952 52.952 52.952

52.9519 52.9519 52.9519

-1.1841
52.9519 -1.1839 -1.1837 -1.1835 -1.1841
52.9519 -1.1839 -1.1837 -1.1835 -1.1841
52.9519 -1.1839 -1.1837 -1.1835
-1.1841 -1.1839 longitude/()-1.1837 -1.1835 -1.1841 -1.1839 longitude/()-1.1837 -1.1835 -1.1841 -1.1839 longitude/()-1.1837 -1.1835
longitude/() longitude/() longitude/()

(a) (b) (c)

(a) (b) (c)
Figure 13. The trajectories
Thetrajectories with
trajectorieswith
withan added GPS gross error using standard Kalman Filter and RKF.RKF.
(a)
Figure13.
Figure 13.The anan added
added GPS
GPS gross
gross error
error using
using standard
standard KalmanKalman Filter
Filter and and
RKF. (a)
one gap
(a) one at the times of 486,420 s; (b) one gap at the times of 486,640 s; (c) one gap at the times of
one gapgap at the
at the times
times of 486,420
of 486,420 s; (b)
s; (b) oneone gap
gap at at
thethe times
times ofof486,640
486,640s;s;(c)
(c)one
onegap
gapatatthe
thetimes
timesof
of
486,820
486,820 s.
s.
486,820 s.
position error
4 position error
4
2 KALMAN FILTER ROBUST KALMAN FILTER
north/m

2 KALMAN FILTER ROBUST KALMAN FILTER

north/m

0
0
-2
-2
-4
486100
-4 486400 486700 487000
486100
4 486400 486700 487000
4
2
east/m

2
east/m

0
0
-2
-2
-4
486100
-4 486400 486700 487000
486100
4 486400 486700 487000
4
2
down/m

2
down/m

0
0
-2
-2
-4
486100
-4 486400 486700 487000
486100 486400 486700 487000
time/s
time/s
Figure 14. The position accuracy comparison between standard Kalman Filter and RKF with an added
Figure
Figure14.
14.The position
Thegross
positionaccuracy
accuracycomparison
comparisonbetween
betweenstandard
standardKalman
KalmanFilter
Filterand
andRKF
RKFwith
withan
anadded
added
pseudo-range error.
pseudo-range
pseudo-range gross
gross error.
error.
Table 6. Comparison between standard Kalman Filter and RKF in term of positioning accuracy with
Table 6. Comparison between standard Kalman Filter and RKF in term of positioning accuracy with
an added pseudo-range gross error.
an added pseudo-range gross error.
Standard KF (m) RKF (m)
Epochs Standard KF (m) RKF (m)
Epochs North East Down North East Down
North East Down North East Down
486420s 0.54 0.37 1.11 0.22 0.10 0.57
486420s 0.54 0.37 1.11 0.22 0.10 0.57
486640s 1.26 0.44 2.38 0.30 0.11 0.78
486640s 1.26 0.44 2.38 0.30 0.11 0.78
486820s 1.66 1.17 0.70 0.49 0.44 0.57
486820s 1.66 1.17 0.70 0.49 0.44 0.57
Sensors 2016, 16, 944 14 of 16

Table 6. Comparison between standard Kalman Filter and RKF in term of positioning accuracy with
an added pseudo-range gross error.

Standard KF (m) RKF (m)

Epochs
North East Down North East Down
486420s 0.54 0.37 1.11 0.22 0.10 0.57
486640s 1.26 0.44 2.38 0.30 0.11 0.78
486820s 1.66 1.17 0.70 0.49 0.44 0.57

Figure 15 reveals the trajectories calculated when a gross error of 2 m is added onto a UWB range
measurement and the corresponding position error is illustrated in Figure 16. At epoch 486,640 s
in Table 7, RKF gives 0.40 m, 0.16 m, and 1.00 m accuracies in the north, east and height directions,
respectively, as well as they are improvements of 23%, 50%, and 49% compared with the result of
standard Kalman Filter. As a result, RKF not only limits the effect of pseudo-range gross error, but also
works fine16,
Sensors 2016, on944
the UWB ranging gross error. 14 of 16
Sensors 2016, 16, 944 14 of 16
trajectory trajectory trajectory
52.9521 trajectory 52.9521 trajectory 52.9521 trajectory
52.9521 KALMAN FILTER 52.9521 KALMAN FILTER 52.9521
KALMAN FILTER FILTER
ROBUST KALMAN KALMAN FILTER
ROBUST KALMAN FILTER
ROBUST KALMAN FILTER
REFERENCE ROBUST KALMAN FILTER
REFERENCE
REFERENCE REFERENCE
latitude/()

latitude/()

latitude/()
latitude/()

latitude/()

latitude/()
52.952 52.952 52.952
52.952 52.952 52.952

KALMAN FILTER
KALMAN FILTER FILTER
ROBUST KALMAN
ROBUST
REFERENCE KALMAN FILTER
52.9519 52.9519 52.9519 REFERENCE
-1.1841
52.9519 -1.1839 -1.1837 -1.1835 -1.1841
52.9519 -1.1839 -1.1837 -1.1835 -1.1841
52.9519 -1.1839 -1.1837 -1.1835
-1.1841 -1.1839longitude/()-1.1837 -1.1835 -1.1841 -1.1839longitude/()-1.1837 -1.1835 -1.1841 -1.1839 longitude/()-1.1837 -1.1835
longitude/() longitude/() longitude/()

(a) (b) (c)

(a) (b) (c)
Figure
Figure15.
15.TheThetrajectories
trajectorieswith
withanan
added
added UWB
UWB gross error
gross using
error standard
using standardKalman Filter
Kalman and and
Filter RKF. (a)
Figure 15. The trajectories with an added UWB gross error using standard Kalman Filter and RKF.RKF.
(a)
one gapgap
(a) one at the times
at the of 486,420
times s; (b)
of 486,420 one
s; (b) gap
one at at
gap the times ofof486,640
486,640s; (c) one onegap
gapat the
thetimes
timesof
one gap at the times of 486,420 s; (b) one gap at thethe times
times of 486,640 s;s;(c)(c)one gap atatthe times of
of
486,820
486,820s.
486,820 s.s.
position error
4 position error
4 KALMAN FILTER ROBUST KALMAN FILTER
2 KALMAN FILTER ROBUST KALMAN FILTER
north/m

2
north/m

0
0
-2
-2
-4
486100
-4 486400 486700 487000
486100
4 486400 486700 487000
4
2
east/m

2
east/m

0
0
-2
-2
-4
486100
-4 486400 486700 487000
486100
4 486400 486700 487000
4
2
down/m

2
down/m

0
0
-2
-2
-4
486100
-4 486400 486700 487000
486100 486400 486700 487000
time/s
time/s

Figure
Figure16.
16.The
Theposition
positionaccuracy
accuracycomparison
comparisonbetween
betweenstandard
standardKalman
KalmanFilter
Filterand
andRKF
RKFwith
withananadded
added
Figure 16. The position accuracy comparison between standard Kalman Filter and RKF with an added
UWB
UWB ranging
ranging gross
gross error.
error.
UWB ranging gross error.

Table 7. Comparison between standard Kalman Filter and RKF in term of positioning accuracy with
Table 7. Comparison between standard Kalman Filter and RKF in term of positioning accuracy with
an added UWB ranging gross error.
an added UWB ranging gross error.
Standard KF (m) RKF (m)
Epochs Standard KF (m) RKF (m)
Epochs North East Down North East Down
North East Down North East Down
486420s 0.34 2.54 0.42 0.22 0.10 0.57
486420s 0.34 2.54 0.42 0.22 0.10 0.57
486640s 0.52 0.32 1.96 0.40 0.16 1.00
486640s 0.52 0.32 1.96 0.40 0.16 1.00
486820s 3.36 1.97 0.20 0.49 0.44 0.57
486820s 3.36 1.97 0.20 0.49 0.44 0.57
Sensors 2016, 16, 944 15 of 16

Table 7. Comparison between standard Kalman Filter and RKF in term of positioning accuracy with
an added UWB ranging gross error.

Standard KF (m) RKF (m)

Epochs
North East Down North East Down
486420s 0.34 2.54 0.42 0.22 0.10 0.57
486640s 0.52 0.32 1.96 0.40 0.16 1.00
486820s 3.36 1.97 0.20 0.49 0.44 0.57

6. Conclusions
This paper has proposed a cooperative positioning scheme supported by V2I communication.
A tightly-coupled GPS/UWB/INS algorithm based on a Robust Kalman Filter (RKF) is illustrated
for the implement of proposed cooperative positioning scheme. Additionally, a gross error detection
procedure is embedded at the beginning of RKF which reliefs the calculation burden of the algorithm.
Six different scenarios are simulated to comprehensively evaluate the performance of the algorithm. It
is shown that the compensation of UWB into GPS/INS can improve the reliability and precision of
the cooperative positioning scheme and also could achieve overall sub-metre accuracy. Particularly,
UWB range measurement remains the positioning availability when there are less than four visible
GPS satellites. Therefore, the characteristics of kinematic communication node should be examined.
Positioning with multi-sensor integration has achieved encouraging results in recent years.
However, it cannot always meet the requirements particularly in the urban canyon environment.
Benefiting from the emerging of V2X communication, cooperative positioning opens another way
to support numerous V2X applications. A cooperative positioning scheme supported by V2I
communication (i.e., stationary node) has been proposed in the present paper and this method will be
extended to V2V communication further to V2X communication. The GPS carrier-phase integration
and more sensory sources are considered as the future work. At the same time, more sensors mean
more redundant information. Useful information selecting and efficient filtering algorithms are very
key issues for multi-sensor fusion.

Acknowledgments: This work was partially supported by the Fundamental Research Funds for the Central
Universities under grant number 2014ZDPY15, the Program for New Century Excellent Talents in University
under grant number NCET-13-1019, a project funded by the Priority Academic Program Development of Jiangsu
Higher Education Institutions and the Cooperative Innovation Center of Jiangsu Province.
Author Contributions: The corresponding author Jian Wang proposed the research, organized the entire
experimental program, and drafted the manuscript. Yang Gao and Zengke Li and were responsible for field data
collection and drafted part of the manuscript. Xiaoling Meng and Craig Hancock performed the data analysis and
were involved in the writing of the manuscript.
Conflicts of Interest: The authors declare no conflicts of interest.

References
1. Garello, R.; Lo Presti, L.; Corazza, G.; Samson, J. Peer-to-Peer Cooperative Positioning Part I:
GNSS Aided Acquisition. Available online: http://www.insidegnss.com/auto/marapr12-WP.pdf
(accessed on 15 March 2012).
2. Fujii, S.; Fujita, A.; Umedu, T.; Kaneda, S.; Yamaguchi, H.; Higashino, T.; Takai, M. Cooperative vehicle
positioning via V2V communications and onboard sensors. In Proceedings of the 2011 IEEE Vehicular
Technology Conference, San Francisco, CA, USA, 58 September 2011.
3. Deambrogio, L.; Palestini, C.; Bastia, F.; Gabelli, G.; Corazza, G.E.; Samson, J. Impact of high-end receivers in
a peer-to-peer cooperative localization system. In Proceedings of the International Conference on Ubiquitous
Positioning, Indoor Navigation and Location-Based Service, Kirkkonummi, Finland, 1415 October 2010.
4. Tsai, M.-F.; Wang, P.-C.; Shieh, C.-K.; Hwang, W.-S.; Chilamkurti, N.; Rho, S.; Lee, Y.S. Improving positioning
accuracy for VANET in real city environments. J. Supercomput. 2014, 71, 19751995. [CrossRef]
Sensors 2016, 16, 944 16 of 16

5. Liu, K.; Lim, H.B. Positioning accuracy improvement via distributed location estimate in cooperative
vehicular networks. In Proceedings of the 2012 15th International IEEE Conference on Intelligent
Transportation Systems, Anchorage, AK, USA, 1619 September 2012.
6. Wang, D.; OKeefe, K.; Petovello, M.G. Decentralized cooperative navigation for vehicle-to-vehicle (V2V)
applications using GPS integrated with UWB range. In Proceedings of the ION Pacific PNT 2013 Conference,
Honolulu, HI, USA, 2225 April 2013.
7. Petovello, M.G.; OKeefe, K.; Chan, B.; Spiller, S.; Pedrosa, C.; Xie, P.; Basnayake, C. Demonstration of
inter-vehicle UWB ranging to augment DGPS for improved relative positioning. J. Glob. Position. Syst. 2012,
11, 1121. [CrossRef]
8. Alam, N.; Balaei, A.T.; Dempster, A.G. A DSRC Doppler-based cooperative positioning enhancement for
vehicular networks with GPS availability. IEEE Trans. Veh. Technol. 2011, 60, 44624470. [CrossRef]
9. Stephenson, S.; Meng, X.; Moore, T.; Baxendale, A.; Edwards, T. A fairy tale approach to cooperative vehicle
positioning. In Proceedings of the 2014 International Technical Meeting of The Institute of Navigation
(ION ITM 2014), San Diego, CA, USA, 2729 January 2014.
10. Chiu, D.S.; MacGougan, G.; OKeefe, K. UWB assisted GPS RTK in hostile environments. In Proceedings of
the ION NTM 2008, San Diego, CA, USA, 2830 January 2008.
11. MacGougan, G.; OKeefe, K.; Chiu, D. Multiple UWB range assisted GPS RTK in hostile environments. In
Proceedings of the 21st International Technical Meeting of the Satellite Division of The Institute of Navigation
(ION GNSS 2008), Savannah, GA, USA, 1619 September 2008.
12. MacGougan, G.; OKeefe, K.; Klukas, R. Tightly-coupled GPS/UWB integration. J. Navig. 2010, 63, 122.
[CrossRef]
13. Tanigawa, M.; Hol, J.D.; Dijkstra, F.; Luinge, H.; Slycke, P. Augmentation of low-cost GPS/MEMS INS with
UWB positioning system for seamless outdoor/indoor positioning. In Proceedings of the 21st International
Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS 2008), Savannah, GA,
USA, 1619 September 2008.
14. Karagiannis, G.; Altintas, O.; Ekici, E.; Heijenk, G.; Jarupan, B.; Lin, K.; Weil, T. Vehicular networking: A
survey and tutorial on requirements, architectures, challenges, standards and solutions. IEEE Commun. Surv.
Tutor. 2011, 13, 584616. [CrossRef]
15. Li, Z.; Wang, J.; Gao, J.; Li, B.; Zhou, F. A vondrak low pass filter for IMU sensor initial alignment on a
disturbed base. Sensors 2014, 14, 2380323821. [CrossRef] [PubMed]
16. Han, S.; Wang, J. Integrated GPS/INS navigation system with dual-rate Kalman Filter. GPS Solut. 2012, 16,
389404. [CrossRef]
17. Wang, J.; Lee, H.; Hewitson, S.; Lee, H.-K. Influence of dynamics and trajectory on integrated GPS/INS
navigation performance. J. Glob. Position. Syst. 2003, 2, 109116. [CrossRef]
18. Li, Z.; Wang, J.; Li, B.; Gao, J.; Tan, X. GPS/INS/Odometer integrated system using fuzzy neural network for
land vehicle navigation applications. J. Navig. 2014, 67, 967983. [CrossRef]
19. Zhang, Y.; Gao, Y. Integration of INS and un-differenced GPS measurements for precise position and attitude
determination. J. Navig. 2008, 61, 8797. [CrossRef]
20. Farrell, J. Aided Navigation: GPS with High Rate Sensors; McGraw-Hill Companies: New York, NY, USA, 2008.
21. Yang, Y.; Cui, X.; Gao, W. Adaptive integrated navigation for multi-sensor adjustment outputs. J. Navig.
2004, 57, 287295. [CrossRef]
22. Yang, Y.; Gao, W. An optimal adaptive Kalman filter. J. Geod. 2006, 80, 177183. [CrossRef]
23. Kealy, A.; Retscher, G.; Toth, C.; Hasnur-Rabiain, A.; Gikas, V.; Grejner-Brzezinska, D.; Danezis, C.; Moore, T.
Collaborative navigation as a solution for PNT applications in GNSS challenged environmentsreport on
field trials of a joint FIG/IAG working group. J. Appl. Geod. 2015, 9, 244263. [CrossRef]

2016 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access
article distributed under the terms and conditions of the Creative Commons Attribution
(CC-BY) license (http://creativecommons.org/licenses/by/4.0/).