A Tightly-Coupled GPS/INS/UWB Cooperative Positioning Sensors System Supported by V2I Communication

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.


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 Section 3 demonstrates the technical structure of proposed cooperative positioning scheme. Then, the Robust Kalman Filter (RKF) is presented through enumerating the observation equation involving GPS, INS and UWB measurements at the Section 4. Finally, experimental tests and discussions are described in Section 5 followed by some conclusions in Section 6.

Cooperative Positioning for V2X Application
The V2X communication architecture as a realisation of the cooperative positioning has been 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 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 and surrounding vehicles via VANET, with respect to the current time. Simultaneously, the unit transmits its own measurements and the calculated position to the neighbouring vehicles. Consequentially, each vehicle positions and sends data recurrently when they are running on the road. GPS, INS and UWB measurements at the Section 4. Finally, experimental tests and discussions are described in Section 5 followed by some conclusions in Section 6.

Cooperative Positioning for V2X Application
The V2X communication architecture as a realisation of the cooperative positioning has been 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 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 and surrounding vehicles via VANET, with respect to the current time. Simultaneously, the unit transmits its own measurements and the calculated position to the neighbouring vehicles. Consequentially, each vehicle positions and sends data recurrently when they are running on the road. The V2X concept is illustrated that vehicles driven in an urban canyon are assumed to receive measurements from not only neighbouring vehicles but also infrastructure points. These data are then used to calculate the vehicles' positions. By taking advantages of V2X communication, vehicles are able to share and exchange information with surrounding transportation participants for important safety applications, such as lane change assistance, intersection collision warning, overtaking vehicle warning, rear end collision warning, etc. [14].
However, a vehicle cannot always resolve its position when only GPS and UWB measurements are obtained from above V2X communication architecture. The reasons could be the poor performance of the GPS/UWB geometry, the limited operational range of the UWB radios and a complete blockage of GPS signals, etc. To further enhance the availability and reliability of the cooperative positioning system, an Inertial Navigation System (INS) needs to be embedded into the GPS and UWB integration positioning system and implementing algorithms need to be developed.

Tightly-Coupled GPS/UWB/INS Cooperative Positioning
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 measurements of accelerometer and gyroscope are fed into and Robust Kalman Filter (RKF) together with UWB range measurements and the GPS coordinates of the infrastructure points and the 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 of a positioning cycle. The V2X concept is illustrated that vehicles driven in an urban canyon are assumed to receive measurements from not only neighbouring vehicles but also infrastructure points. These data are then used to calculate the vehicles' positions. By taking advantages of V2X communication, vehicles are able to share and exchange information with surrounding transportation participants for important safety applications, such as lane change assistance, intersection collision warning, overtaking vehicle warning, rear end collision warning, etc. [14].
However, a vehicle cannot always resolve its position when only GPS and UWB measurements are obtained from above V2X communication architecture. The reasons could be the poor performance of the GPS/UWB geometry, the limited operational range of the UWB radios and a complete blockage of GPS signals, etc. To further enhance the availability and reliability of the cooperative positioning system, an Inertial Navigation System (INS) needs to be embedded into the GPS and UWB integration positioning system and implementing algorithms need to be developed.

Tightly-Coupled GPS/UWB/INS Cooperative Positioning
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 measurements of accelerometer and gyroscope are fed into and Robust Kalman Filter (RKF) together with UWB range measurements and the GPS coordinates of the infrastructure points and the 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 of a positioning cycle. 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 of positioning capability. INS feeds its calculated pseudo-range and Doppler measurement into 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 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.

Dynamic Equation
The system error of the dynamics model of integrated navigation used in the Kalman Filter is designed based on the INS error equations. The insignificant terms are neglected in the process of linearization [15]. The psi-angle error equations of INS are as follows [16]: where  r ,  v and  ψ are the position, velocity and orientation error vectors, respectively. en ω is the rate of navigation frame with respect to earth, and ie ω is the rate of earth with respect to inertial frame. Besides, f is the force observation. Then, the system error dynamics of GPS/INS integration is obtained by expanding the accelerometer bias error vector η and the gyro drift error vector ε .
The accelerometer bias error vector η and the gyro drift error vector ε are regarded as the random walk process vectors, which are modelled as follows [17]: where  u and  u are white Gaussian noise vectors.
The receiver clock state dynamic equations can be written as follows: 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 of positioning capability. INS feeds its calculated pseudo-range and Doppler measurement into 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 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.

Dynamic Equation
The system error of the dynamics model of integrated navigation used in the Kalman Filter is designed based on the INS error equations. The insignificant terms are neglected in the process of linearization [15]. The psi-angle error equations of INS are as follows [16]: where δr, δv and δψ are the position, velocity and orientation error vectors, respectively. ω en is the rate of navigation frame with respect to earth, and ω ie is the rate of earth with respect to inertial frame. Besides, f is the force observation. Then, the system error dynamics of GPS/INS integration is obtained by expanding the accelerometer bias error vector η and the gyro drift error vector ε. The accelerometer bias error vector η and the gyro drift error vector ε are regarded as the random walk process vectors, which are modelled as follows [17]: .
where u η and u ε are white Gaussian noise vectors.
The receiver clock state dynamic equations can be written as follows: where u dt 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.

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]: 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 r UWB i is the UWB range measurements of the ith UWB unit [20]: (10) where x UWB , y UWB and z UWB 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: where H k is the observation matrix [20], and τ is the measurement noise vector with covariance matrix R k , assumed to be white Gaussian noise.

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: The observation update equation of Kalman Filter is expressed as: where X k is a priori state estimation,X k is a posteriori state estimation, G k is the gain matrix of Kalman Filter, C k is a priori covariance matrix of state vector, C k´1 is a posteriori covariance matrix of state vector, R k is the covariance matrix of measurement noise vector, Q k´1 is the covariance matrix of process noise, and F k,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]: where k 0 and k 1 are constants which have the values k 0 = 2.5~3.5 and k 1 = 3.5~4.5, respectively. ( 19) where V k and d k 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: ( 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.

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. 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 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 and each of them was equipped with a set of equipment that included a GPS receiver, a UWB unit and a battery. The product type of the UWB unit is Lock-on Model LD2 provided by Thales Research & Technology Ltd., West Sussex, UK. The unit complies with the standard UWB power density limit of´41.3 dBm/MHz, which leads to a nominal output power of 100 µW. The transmission is Frequency Hopped Direct Sequence Spread Spectrum signal covering: 4760 MHz to 6200 MHz. The range accuracy of decimetre level can be achieved by UWB. To calibrate the GPS and the UWB antenna precisely, the GPS pillar fastened with UWB was held vertically as stable as possible during the tests. Another GPS receiver was set on one of the pillars on the NGI roof to act as the reference station. As the UWB ranges were time tagged by the laptop system time, the laptop was synchronized to GPS time to collect 1 Hz UWB ranges prior to the test. Before starting the tests, we assume that the locomotive and two people were carrying a DSRC device within the reachable communication range. The sampling rate of GPS receivers and IMU were respectively configured as 10 Hz and 200 Hz to simulate a running vehicle under V2I communication environment. The locomotive ran along the track to act as a vehicle that is able to exchange raw observations and distributed positioning coordinates with surrounding traffic participants (thee two people). Therefore, a V2I network consists of a running vehicle, two participants and a reference station is plotted in Figure 3a and the devices used during the tests are presented in Figure 3b. Therefore, a V2I network consists of a running vehicle, two participants and a reference station is plotted in Figure 3a and the devices used during the tests are presented in Figure 3b. A GPS software tool called GrafNav TM (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 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  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 PDOP value almost approaches to 20 while the PDOP value of the cooperative positioning still remains less than 5 as shown in Figure 4b. It is illustrated that the fusion of the GPS/UWB/INS raw observations obtained via V2I communication network can greatly improve the reliability of positioning capability.

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 is estimated. As shown in Figure 5a, the delay of the autocorrelation estimate of the

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 is estimated. As shown in Figure 5a, the delay of the autocorrelation estimate of the predicted residuals is zero and the variance of the residual is 0.14 m 2 . In Figure 5b, the absolute values of the residuals of the UWB measurements are all less than 0.2 m that proves the relationship among the data is low. As shown in Figure 6, the predicted residuals approximately coincide with the Gaussian distribution and the variance of 0.14 m 2 is then used as the value of the stochastic parameter in the mentioned Robust Kalman Filter (RKF) model. 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  As shown in Figure 6, the predicted residuals approximately coincide with the Gaussian distribution and the variance of 0.14 m 2 is then used as the value of the stochastic parameter in the mentioned Robust Kalman Filter (RKF) model. As shown in Figure 6, the predicted residuals approximately coincide with the Gaussian distribution and the variance of 0.14 m 2 is then used as the value of the stochastic parameter in the mentioned Robust Kalman Filter (RKF) model. 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 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.

GPS/INS/UWB Cooperative Positioning
As seen in Figure 7, the trajectory of GPS/INS integration has a systematic bias mainly caused by the errors in the pseudo-range and Doppler observations. The reasons for this could be the satellite obit errors, clock errors atmospheric errors, etc. Figure 8 and Table 3 show that the positioning accuracies (RMS) in the north, east and down are improved 76%, 61%, and 16%, respectively.

GPS/INS/UWB Cooperative Positioning
As seen in Figure 7, the trajectory of GPS/INS integration has a systematic bias mainly caused by the errors in the pseudo-range and Doppler observations. The reasons for this could be the satellite obit errors, clock errors atmospheric errors, etc. Figure 8 and Table 3 show that the positioning accuracies (RMS) in the north, east and down are improved 76%, 61%, and 16%, respectively.

GPS/INS/UWB Cooperative Positioning
As seen in Figure 7, the trajectory of GPS/INS integration has a systematic bias mainly caused by the errors in the pseudo-range and Doppler observations. The reasons for this could be the satellite obit errors, clock errors atmospheric errors, etc. Figure 8 and Table 3 show that the positioning accuracies (RMS) in the north, east and down are improved 76%, 61%, and 16%, respectively.     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 by integrating UWB observations. By integrating UWB range measurements, the cooperative positioning accuracies in north, east and down are improved by 73%, 76%, and 33%, respectively, in Figure 10 and Table 4. It is also noticed that the improvements of accuracy in the case of only 4 satellites are superior to the case of more than 4 satellites after integrating the UWB observation.

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 in Figure 12. In the case of less than 4 satellites, the positioning errors enlarge with time and the INS errors cannot be estimated correctly. The integration of UWB range measurement can drag the trajectory back to the right way and remain relative high accuracy. As listed in Table 5

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.         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 fine on the UWB ranging gross error.

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.