A Tightly-Coupled GPS/INS/UWB Cooperative ... - Semantic Scholar

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 2 3 4

*

School of Environmental Science and Spatial Informatics, China University of Mining and Technology, Xuzhou 221116, China; [email protected] (J.W.); [email protected] (Z.L.) Nottingham Geospatial Institute, The University of Nottingham, Nottingham NG7 2TU, UK; [email protected] Sino-UK Geospatial Engineering Centre, The University of Nottingham, Nottingham NG7 2TU, UK Department of Civil Engineering, University of Nottingham, Ningbo 315100, China; [email protected] Correspondence: [email protected]; 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.1–10.6 Hz within the measurement systems domain. Due to UWB’s 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 [10–12]. 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 vehicle’s 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 Kalman Filtermeasurements (RKF) is presented enumerating the observation equation involving GPS, INS and UWB at thethrough Section 4. Finally, experimental tests and discussions are GPS, INS and UWB 5measurements at theconclusions Section 4. in Finally, experimental tests and discussions are described in Section followed by some Section 6. 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 V2X as aarealisation positioning has been developed for communication several years. It architecture is assumed that Vehicular of Adthe hoc cooperative Network (VANET) is established developed for several years. It is assumed that a Vehicular Ad hoc Network (VANET) is established based on V2X communication, as shown in Figure 1. The protocol that demonstrates how a vehicle based onits V2X communication, as shown in Figure Thepresented protocol that demonstrates resolves position through the proposed scheme1.are as follows: Firstly,how eacha vehicle vehicle resolves its position through the proposed scheme are presented as follows: Firstly, each vehicle positioning unit calculates its own position using available data collected from both equipped sensors positioning unit calculates its VANET, own position using available data collected from both equipped and surrounding vehicles via with respect to the current time. Simultaneously, the unit sensors and surrounding vehicles via VANET, with respect to the current time. Simultaneously, transmits its own measurements and the calculated position to the neighbouring vehicles. the unit transmitseach its own measurements and the calculated position towhen the neighbouring vehicles. Consequentially, vehicle positions and sends data recurrently they are running on Consequentially, each vehicle positions and sends data recurrently when they are running on the road. 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 is illustrated that vehicles driven an urban canyon are assumed receive measurements from not only neighbouring vehicles butinalso infrastructure points. Thesetodata are measurements from not only neighbouring vehicles but also infrastructure points. These data are then then used to calculate the vehicles’ positions. By taking advantages of V2X communication, vehicles used to calculate vehicles’ positions. By taking advantages of V2X communication, vehicles are are able to sharetheand exchange information with surrounding transportation participants for able to share and exchange information with surrounding transportation participants for important safety applications, such as lane change assistance, intersection collision important warning, safety applications, such as lane assistance, intersection collision warning, overtaking vehicle overtaking vehicle warning, rearchange end collision warning, 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 a vehicle cannot resolve its position when only and UWB are obtained above V2Xalways communication architecture. The GPS reasons couldmeasurements be the poor are obtained from above V2X communication architecture. The reasons could be the poor performance performance of the GPS/UWB geometry, the limited operational range of the UWB radios and a of the GPS/UWB geometry, the limited operational range of the UWB radios and a complete blockage complete blockage of GPS signals, etc. To further enhance the availability and reliability of the of GPS signals, etc. To further enhance the availability reliability of theto cooperative positioning cooperative positioning system, an Inertial Navigation and System (INS) needs be embedded into the system, an Inertial Navigation System (INS) needs to be embedded into the GPS and UWB integration GPS and UWB integration positioning system and implementing algorithms need to be developed. 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 of16, a positioning cycle. Sensors 2016, 944 4 of 16

Figure method. Figure 2. 2. The The schematic schematic of of proposed proposed cooperative cooperative positioning 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 of the the dynamics dynamics model model of of integrated integrated navigation navigation used the Kalman Kalman Filter The system system error error of used in in the Filter is is designed based on the INS error equations. The insignificant terms are neglected in the process designed based on the INS error equations. The insignificant terms are neglected in the process of of linearization [15]. The equations of of INS INS are are as as follows follows [16]: [16]: linearization [15]. The psi-angle psi-angle error error equations .  r  ωen   r   v δr “ ´ωen ˆ δr ` δv  v  (2ωie  ωen )   v   ψ  f  η . δv “ ´p2ωie ` ωen q ˆ δv ´ δψ ˆ f ` η   (ωie  ωen )   ψ  ε . ψ δψ “ ´pωie ` ωen q ˆ δψ ` ε

(1) (1) (2) (2) (3) (3)

 rδv  v and where δr, , and  ψtheare the position, and orientation errorrespectively. vectors, respectively. ωen where δψ are position, velocityvelocity and orientation error vectors, ωen is the rate is rate of navigation frame withtorespect earth, is of theearth rate of earth with respect to inertial ofthe navigation frame with respect earth,to and ωie and is theωrate with respect to inertial frame. ie Besides,Besides, f is the force observation. Then, the system dynamics of GPS/INS integration is obtained frame. f is the force observation. Then, theerror system error dynamics of GPS/INS integration is by expanding the accelerometer bias errorbias vector η and the gyro drift ε. vector ε . η and obtained by expanding the accelerometer error vector theerror gyrovector drift error The accelerometer η and the gyro drift drift errorerror vectorvector ε are regarded as the random η and ε are regarded The accelerometerbias biaserror errorvector vector the gyro 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 white Gaussian noise vectors. where uuηand are white Gaussian noise vectors.  are The receiver clock state dynamic equations can be written as follows:

   dt  u dt dt

(6)

 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 .

δdt “ uδdt

(6) (7)

where udt and uδdt 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 fi Pj ´ PjINS — GPS ffi — D j ´ DINS ffi j — ffi Z “ — UWB (9) INS ffi ´ ri – ri fl .. . where PjGPS and DGPS are the pseudo-range and Doppler value observed by the jth GPS satellite, j respectively; PjINS and DINS are the pseudo-range and Doppler measurement of the jth satellite j 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,k´1 Xˆ k´1 (12) T C k “ Fk,k´1 Ck´1 Fk,k ´1 ` Qk´1

(13)

Sensors 2016, 16, 944

6 of 16

The observation update equation of Kalman Filter is expressed as: Vk “ Zk ´ Hk X k

(14)

Xˆ k “ X k ` Gk Vk ˆ ˙ ´1 1 T T Gk “ C k Hk Hk C k Hk ` Rk γk

(15)

Ck “ pI ´ Gk Hk q C k

(16) (17)

where X k is a priori state estimation, Xˆ k is a posteriori state estimation, Gk is the gain matrix of Kalman Filter, C k is a priori covariance matrix of state vector, Ck´1 is a posteriori covariance matrix of state vector, Rk is the covariance matrix of measurement noise vector, Qk´1 is the covariance matrix of process noise, and Fk,k´1 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 γk “ ˆ , k0 ă sk ď k1 sk k 1 ´k 0 ’ ’ % 10´30 , s ą k k

(18)

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 Scale factor Random walk Sampling Rate

1˝ /h

1.0 mg 300 ppm 6 mg/sqrt(Hz)

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

(a)

(b) Figure 3. 3. The The test testscenario scenariofor forverifying verifyingthe theproposed proposed architecture. Test deployment diagram; (b) Figure architecture. (a)(a) Test deployment diagram; (b) the the locomotive and GPS and the UWB unit. locomotive and GPS and the 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, value Sensors 16,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 thanvalue 5 as of shown in Figure 4b. It is illustrated that the fusion the GPS/UWB/INS raw while theless PDOP the cooperative positioning still remains less than of 5 as shown in Figure 4b. It observations obtained via of V2I can greatly improve thecommunication reliability of is illustrated that the fusion thecommunication GPS/UWB/INS network raw observations obtained via V2I positioning network cancapability. greatly improve the reliability of positioning capability. PDOP

1

0 486100

486400

486700

487000

PDOP

20

GPS/INS/UWB GPS/INS

GPS/INS/UWB GPS/INS

PDOP

PDOP

2

10

0 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 static static kinematic

Scenarios kinematic

Horizontal Vertical Horizontal 5 mm + 0.5 ppm 10 mm + 0.5Vertical ppm mm++10.5 ppm 20 mm10 + 0.5 ppm 10 5mm ppm + 1mm ppm 10 mm + 1 ppm

20 mm + 1 ppm

To overall evaluate the performance of the cooperative positioning scheme, six scenarios are considered. Theevaluate average velocity of the moving frompositioning all experience is about m/s: To overall the performance of theplatform cooperative scheme, six2 scenarios 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 for of cooperative in the caseDoppler of more observations than four satellites. Scenario 1: Integration GPS/INS positioning using pseudo-ranges, 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 3: Integration of GPS/INS using and rawranges INS Scenario 4: Integration of GPS/UWB/INS usingpseudo-ranges, pseudo-ranges,Doppler Dopplerobservations observations, UWB 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 of GPS/UWB/INS pseudo-ranges, Doppler observations, Scenario 5: Integration of GPS/UWB/INS using using pseudo-ranges, Doppler observations, UWBUWB ranges ranges raw INS observations for cooperative in three, the case of and onlyone and raw INSand observations for cooperative positioning positioning in the case of two four satellites. visible satellites. Scenario 6: Integration of GPS/UWB/INS using using pseudo-ranges, Doppler observations, UWBUWB ranges Scenario 5: Integration of GPS/UWB/INS pseudo-ranges, Doppler observations, and raw INSand observations in the case of errors. positioning in the case of three, two ranges raw INS observations forgross 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 parameters of the UWB measurements, autocorrelation function and ranges and raw INS observations in the 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

70 70 60 60 50 50 40 40 30 30 20 20 10 10 0 00 0

1 1 UWB Residuals(m) UWB Residuals(m)

Amplitude Amplitude

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, estimated. As shown in Figure 5a, the delay of the autocorrelation estimate of the predicted Sensors 2016, 944 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 of the UWB measurements are all less than 0.2 m thatthe proves the relationship among residuals of the UWB are all less 0.2than m that relationship among the data of the residuals of themeasurements UWB measurements are than all less 0.2proves m that proves the relationship among thelow. data is low. is the data is low.

0.2 0.2

0.4 0.6 0.4 0.6 Frequency(Hz) Frequency(Hz)

0.8 0.8

0.5 0.5 0 0 -0.5 -0.5 -1 -1 0 0

1 1

200 200

400 400 time(s) time(s)

(a) (a)

600 600

800 800

(b) (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 of the UWB signal; (b) the residuals of the UWB measurement. characteristics characteristics of of the the UWB UWB signal; signal; (b) (b) the the residuals residuals of of the the UWB UWB measurement. measurement.

0.2 0.2 0.1 0.1 0 0 -0.1 -0.1 -0.2 -0.2 -1000 -1000

-500 0 500 -500Delay 0time/s500 Delay time/s

1000 1000

Autocorrelation (coeff) Autocorrelation (coeff)

Autocorrelation (unbiased) Autocorrelation (unbiased)

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 used as as thethe value of the stochastic parameter in the and the variance of 0.14 m is then used value of the stochastic parameter in mentioned Robust Kalman Filter (RKF) model. mentioned Robust Kalman Filter (RKF) model. the mentioned Robust Kalman Filter (RKF) model. 1 1 0.5 0.5 0 0 -0.5 -0.5 -1000 -1000

-500 0 500 -500Delay 0time/s500 Delay time/s

1000 1000

Probability Probabilitydensity density

1 1

0.5 0.5

0 -1.5 0 -1.5

-1 -1

-0.5 -0.5

0 0 Residuals/m Residuals/m

0.5 0.5

1 1

1.5 1.5

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 Sensors 2016, 16, 944

10 of 16 10 of 16

5.2. GPS/INS/UWB Cooperative Positioning

5.2. GPS/INS/UWB Cooperative Positioning

5.2. seen GPS/INS/UWB Positioning As in Figure Cooperative 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 the Doppler The The reasons for for thisthis could bebe the As seenpseudo-range ininFigure 7, theand trajectory of observations. GPS/INS integration hasreasons a systematic bias mainly caused by the the pseudo-range and Doppler observations. could thesatellite satelliteobit errors, clock errors atmospheric errors, etc. Figure 8 and Table 3 show that the positioning accuracies byobit the errors, errors inclock the pseudo-range and Doppler observations. The reasons for this could be the satellite errors atmospheric errors, etc. Figure 8 and Table 3 show that the positioning obit errors, clock errors atmospheric errors, etc. Figure 8 and Table 3 show that the positioning (RMS) accuracies in the north, east downeast areand improved 76%, 61%, and (RMS) inand the north, down are improved 76%,16%, 61%,respectively. and 16%, respectively. accuracies (RMS) in the north, east and down are improved 76%, 61%, and 16%, respectively. trajectory trajectory

latitude/(°) latitude/(°)

52.9521 52.9521

GPS/INS/UWB GPS/INS/UWB GPS/INS GPS/INS REFERENCE REFERENCE

52.952 52.952

52.9519 -1.1841 52.9519 -1.1841

-1.1839 -1.1837 -1.1839 longitude/(°) -1.1837 longitude/(°)

-1.1835 -1.1835

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 position error

GPS/INS/UWB GPS/INS GPS/INS/UWB GPS/INS

486400 486400

486700 486700

487000 487000

486400 486400

486700 486700

487000 487000

486400 486400

486700 time/s 486700 time/s

487000 487000

down/m down/m

east/m east/m

north/m north/m

4 42 20 0-2 -2-4 486100 -4 486100 2 21 10 0-1 -1-2 486100 -2 486100 2 21 10 0-1 -1-2 486100 -2 486100

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 North East East Scenario North 1 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)

Scenario

MAX (m)

North

East

Down

North

East

Down

1.82 0.44

0.57 0.22

0.68 0.57

3.30 1.86

1.60 0.70

1.89 1.53

1 2

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 UWB observations. By integrating UWB range measurements, the cooperative Sensors 2016,by 16,integrating 944 11 of 16 positioning accuracies in north, east and down are improved by 73%, 76%, and 33%, respectively, in Figure and Table It is alsotonoticed that improvements of accuracy in the casethe of only of only 4 10 satellites are 4.superior the case ofthe more than 4 satellites after integrating UWB4 satellites are superior to the case of more than 4 satellites after integrating the UWB observation. observation. trajectory


52.9521

GPS/INS/UWB GPS/INS REFERENCE

52.952

52.9519 -1.1841

-1.1839

-1.1837 longitude/(°)

-1.1835

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

north/m north/m

4

east/m east/m

GPS/INS/UWB GPS/INS

0

-4 486100 4

486400

486700

487000

486400

486700

487000

486700

487000

0

-4 486100 4 down/m down/m

position error

0

-4 486100

486400

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.

Scenario 3 4

RMS (m) North East Down 1.75 1.09 1.64 0.48 0.26 1.10

MAX (m) North East Down 3.29 4.59 3.37 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)

Scenario 3 4

MAX (m)

North

East

Down

North

East

Down

1.75 0.48

1.09 0.26

1.64 1.10

3.29 1.86

4.59 0.87

3.37 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 to the right way relative high accuracy. listed in TableAs5, measurement can drag back the trajectory back toand the remain right way and remain relativeAs high 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 during three listed in Table 5, 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 periodsthree whileperiods the accuracies of GPS/INS are beyond oneare meter. As the during while the accuraciessolution of GPS/INS solution beyond onelocomotive meter. As was the 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 loss of the GPS locomotive was running on a flat track, the accuracy of height does not drift away a lot because of locomotive was running on a flat track, the accuracy of height does not drift away a lot because of satellite. addition, the accuracies in the the north, east andinheight directions gradually on the loss ofInthe GPS satellite. In addition, accuracies the north, east and heightdegrade directions the loss of the GPS satellite. In addition, the accuracies in the north, east and height directions account ofdegrade decreasing visible GPS satellite. visible GPS satellite. gradually on account of decreasing gradually degrade on account of decreasing visible GPS satellite.

52.9519 -1.1841 52.9519 -1.1841

-1.1839 -1.1837 -1.1839 longitude/(°)-1.1837 longitude/(°)

-1.1835 -1.1835

52.952 52.952

52.9519 -1.1841 52.9519 -1.1841

(a) (a)


-1.1835 -1.1835

trajectory trajectory

52.9521 52.9521



52.952 52.952


52.9521 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.1841


(b) (b)

(c) (c)

east/m east/m

north/m north/m

Figure 11. The trajectories of the locomotive in three GPS blockage periods (Scenario 5). (a) one gap Figure 11. 11. The Thetrajectories trajectoriesof ofthe the locomotive locomotivein inthree three GPS GPS blockage blockageperiods periods(Scenario (Scenario5). 5).(a) (a)one onegap gap Figure with three satellites; (b) one gap with two satellites; (c) one gap with one satellite. with three satellites; (b) one gap with two satellites; (c) one gap with one satellite. with three satellites; (b) one gap with two satellites; (c) one gap

down/m down/m


52.9521 52.9521

4 4 2 2 0 0 -2 -2 -4 486100 -4 486100 8 8 4 4 0 0 -4 -4 -8 486100 -8 486100 4 4 2 2 0 0 -2 -2 -4 486100 -4 486100

position error position error

GPS/INS/UWB GPS/INS/UWB

GPS/INS GPS/INS

486400 486400

486700 486700

487000 487000

486400 486400

486700 486700

487000 487000

486700 486700

487000 487000

486400 486400

time/s time/s

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

Gaps Gaps 486400s–486450s 486400s–486450s 486600s–486650s

GPS/INS/UWB (m) GPS/INS/UWB (m) North East Down North East Down 0.42 0.17 0.39 0.42 0.17 0.39 0.41 0.18 0.53

GPS/INS (m) GPS/INS (m) North East Down North East Down 0.56 1.34 0.39 0.56 1.34 0.39 1.03 2.65 0.54

-1.1835 -1.1835

Sensors 2016, 16, 944

13 of 16

Table 5. Position errors in three insufficient GPS satellites periods. GPS/INS/UWB (m)

Gaps

GPS/INS (m)

North

East

Down

North

East

Down

0.42 0.41 0.46

0.17 0.18 0.17

0.39 0.53 0.58

0.56 1.03 0.54

1.34 2.65 0.92

0.39 0.54 0.58

486400s–486450s 486600s–486650s 486800s–486850s

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, respectively, comparing against the accuracies of standard Kalman Filter. Sensors 2016, 16, 944 13 of 16

Sensors 2016, 16, 944

trajectory trajectory KALMAN FILTER KALMAN FILTER FILTER ROBUST KALMAN ROBUST KALMAN FILTER REFERENCE REFERENCE


-1.1835 -1.1835

52.952 52.952

52.9519 -1.1841 52.9519 -1.1841


(a) (a)

-1.1835 -1.1835


52.9521 52.9521


52.952 52.952

52.9519 -1.1841 52.9519 -1.1841


52.9521 52.9521


52.9521 52.9521

52.952 52.952

52.9519 -1.1841 52.9519 -1.1841

(b) (b)


(c) (c)

east/m east/m

north/m north/m

Figure 13. The trajectories with an added GPS gross error using standard Kalman Filter and RKF.RKF. (a) Figure13. 13.The Thetrajectories trajectorieswith with added GPS gross error using standard Kalman Filter Figure anan added GPS gross error using standard Kalman Filter and and RKF. (a) one gap at the times of 486,420 s; (b) one gap at the times of 486,640 s; (c) one gap at the times of (a) one at the times of 486,420 s; (b) one gap the times 486,640s;s;(c) (c)one onegap gapatatthe thetimes timesof of one gapgap at the times of 486,420 s; (b) one gap at at the times ofof486,640 486,820 s. 486,820 s. 486,820 s.

down/m down/m


13 of 16

4 4 2 2 0 0 -2 -2 -4 486100 -4 486100 4 4 2 2 0 0 -2 -2 -4 486100 -4 486100 4 4 2 2 0 0 -2 -2 -4 486100 -4 486100

position error position error KALMAN FILTER KALMAN FILTER

ROBUST KALMAN FILTER ROBUST KALMAN FILTER

486400 486400

486700 486700

487000 487000

486400 486400

486700 486700

487000 487000

486700 486700

487000 487000

486400 486400

time/s time/s

Figure 14. The position accuracy comparison between standard Kalman Filter and RKF with an added Figure position accuracy Figure14. 14.The Thegross position 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.

Epochs Epochs 486420s 486420s 486640s 486640s 486820s 486820s

Standard KF (m) Standard KF (m) North East Down North East Down 0.54 −0.37 1.11 0.54 −0.37 1.11 1.26 0.44 2.38 1.26 0.44 2.38 −1.66 1.17 0.70 −1.66 1.17 0.70

North North 0.22 0.22 0.30 0.30 −0.49 −0.49

RKF (m) RKF (m) East Down East Down −0.10 0.57 −0.10 0.57 0.11 0.78 0.11 0.78 0.44 0.57 0.44 0.57

-1.1835 -1.1835

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)

Epochs 486420s 486640s 486820s

RKF (m)

North

East

Down

North

East

Down

0.54 1.26 ´1.66

´0.37 0.44 1.17

1.11 2.38 0.70

0.22 0.30 ´0.49

´0.10 0.11 0.44

0.57 0.78 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, on944 the UWB ranging gross error. Sensors 2016, 14 of 16 Sensors 2016, 16, 944


-1.1839 -1.1837 -1.1839longitude/(°)-1.1837 longitude/(°)

-1.1835 -1.1835

52.952 52.952

52.9519 -1.1841 52.9519 -1.1841

-1.1839 -1.1837 -1.1839longitude/(°)-1.1837 longitude/(°)

(a) (a)


52.9521 52.9521


52.952 52.952

52.9519 -1.1841 52.9519 -1.1841

trajectory trajectory KALMAN FILTER KALMAN FILTER ROBUST KALMAN FILTER ROBUST KALMAN FILTER REFERENCE REFERENCE

52.9521 52.9521


52.9521 52.9521

-1.1835 -1.1835

52.952 52.952

52.9519 -1.1841 52.9519 -1.1841

(b) (b)

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

(c) (c)

east/m east/m

north/m north/m

Figure anan added UWB gross error using standard Kalman Filter and and RKF. (a) Figure15. 15.The Thetrajectories trajectorieswith with added UWB gross error using standard Kalman Filter Figure 15. The trajectories with an added UWB gross error using standard Kalman Filter and RKF.RKF. (a) one gapgap at the times of 486,420 s; (b) one gap at at the times ofof486,640 (c) one the (a) one at the times of 486,420 s; (b) one gap times 486,640s; onegap gapat thetimes timesof of one gap at the times of 486,420 s; (b) one gap at thethe times of 486,640 s;s;(c)(c)one gap atatthe times of 486,820 486,820s. 486,820 s.s.

down/m down/m


14 of 16

4 4 2 2 0 0 -2 -2 -4 486100 -4 486100 4 4 2 2 0 0 -2 -2 -4 486100 -4 486100 4 4 2 2 0 0 -2 -2 -4 486100 -4 486100

position error position error

KALMAN FILTER KALMAN FILTER

ROBUST KALMAN FILTER ROBUST KALMAN FILTER

486400 486400

486700 486700

487000 487000

486400 486400

486700 486700

487000 487000

486700 486700

487000 487000

486400 486400

time/s time/s

Figure Figure16. 16.The Theposition positionaccuracy accuracycomparison comparisonbetween betweenstandard standardKalman KalmanFilter Filterand andRKF RKFwith withan anadded added Figure 16. The position accuracy comparison between standard Kalman Filter and RKF with an added UWB ranging gross error. UWB ranging gross 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.

Epochs Epochs 486420s 486420s 486640s 486640s 486820s 486820s

Standard KF (m) Standard KF (m) North East Down North East Down 0.34 −2.54 0.42 0.34 −2.54 0.42 0.52 0.32 1.96 0.52 0.32 1.96 3.36 −1.97 0.20 3.36 −1.97 0.20

North North 0.22 0.22 0.40 0.40 −0.49 −0.49

RKF (m) RKF (m) East Down East Down −0.10 0.57 −0.10 0.57 0.16 1.00 0.16 1.00 0.44 0.57 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)

Epochs 486420s 486640s 486820s

RKF (m)

North

East

Down

North

East

Down

0.34 0.52 3.36

´2.54 0.32 ´1.97

0.42 1.96 0.20

0.22 0.40 ´0.49

´0.10 0.16 0.44

0.57 1.00 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.

2.

3.

4.

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). 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, 5–8 September 2011. 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, 14–15 October 2010. 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, 1975–1995. [CrossRef]

Sensors 2016, 16, 944

5.

6.

7.

8. 9.

10. 11.

12. 13.

14.

15. 16. 17. 18. 19. 20. 21. 22. 23.

16 of 16

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, 16–19 September 2012. Wang, D.; O’Keefe, 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, 22–25 April 2013. Petovello, M.G.; O’Keefe, 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, 11–21. [CrossRef] 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, 4462–4470. [CrossRef] 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, 27–29 January 2014. Chiu, D.S.; MacGougan, G.; O’Keefe, K. UWB assisted GPS RTK in hostile environments. In Proceedings of the ION NTM 2008, San Diego, CA, USA, 28–30 January 2008. MacGougan, G.; O’Keefe, 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, 16–19 September 2008. MacGougan, G.; O’Keefe, K.; Klukas, R. Tightly-coupled GPS/UWB integration. J. Navig. 2010, 63, 1–22. [CrossRef] 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, 16–19 September 2008. 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, 584–616. [CrossRef] 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, 23803–23821. [CrossRef] [PubMed] Han, S.; Wang, J. Integrated GPS/INS navigation system with dual-rate Kalman Filter. GPS Solut. 2012, 16, 389–404. [CrossRef] 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, 109–116. [CrossRef] 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, 967–983. [CrossRef] Zhang, Y.; Gao, Y. Integration of INS and un-differenced GPS measurements for precise position and attitude determination. J. Navig. 2008, 61, 87–97. [CrossRef] Farrell, J. Aided Navigation: GPS with High Rate Sensors; McGraw-Hill Companies: New York, NY, USA, 2008. Yang, Y.; Cui, X.; Gao, W. Adaptive integrated navigation for multi-sensor adjustment outputs. J. Navig. 2004, 57, 287–295. [CrossRef] Yang, Y.; Gao, W. An optimal adaptive Kalman filter. J. Geod. 2006, 80, 177–183. [CrossRef] 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 environments–report on field trials of a joint FIG/IAG working group. J. Appl. Geod. 2015, 9, 244–263. [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/).