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

^{1}

^{2}

^{3}

^{4}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Cooperative Positioning for V2X Application

## 3. Tightly-Coupled GPS/UWB/INS Cooperative Positioning

## 4. Robust Kalman Filter for Cooperative Positioning

#### 4.1. Dynamic Equation

**f**is the force observation. Then, the system error dynamics of GPS/INS integration is obtained by expanding the accelerometer bias error vector $\eta $ and the gyro drift error vector $\epsilon $.

**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

#### 4.3. Robust Kalman Filter

_{0}and k

_{1}are constants which have the values k

_{0}= 2.5~3.5 and k

_{1}= 3.5~4.5, respectively.

_{k}and d

_{k}are the predicted residual and their reciprocal of the weight of the observation vector, ${\sigma}_{k}$ the variance of the unit weight can be written as:

## 5. Experimental Tests

^{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 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.

- Scenario 1:
- Integration of GPS/INS using pseudo-ranges, Doppler observations and raw INS observations for cooperative positioning in the case of more than four satellites.
- Scenario 2:
- Integration of GPS/UWB/INS using pseudo-ranges, Doppler observations, UWB range and raw INS observations for cooperative positioning in the case of more than four satellites.
- Scenario 3:
- Integration of GPS/INS using pseudo-ranges, Doppler observations and raw INS observations for cooperative positioning in the case of only four satellites.
- Scenario 4:
- Integration of GPS/UWB/INS using pseudo-ranges, Doppler observations, UWB ranges and raw INS observations for cooperative positioning in the case of only four satellites.
- Scenario 5:
- Integration of GPS/UWB/INS using pseudo-ranges, Doppler observations, UWB ranges and raw INS observations for cooperative positioning in the case of three, two and one visible satellites.
- Scenario 6:
- Integration of GPS/UWB/INS using pseudo-ranges, Doppler observations, UWB ranges and raw INS observations in the case of gross errors.

#### 5.1. Stochastic Model of UWB Ranges

^{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.

^{2}is then used as the value of the stochastic parameter in the mentioned Robust Kalman Filter (RKF) model.

#### 5.2. GPS/INS/UWB Cooperative Positioning

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

#### 5.4. Robustness Test of Robust Kalman Filter

## 6. Conclusions

## Acknowledgments

## Author Contributions

## Conflicts of Interest

## References

- 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. [Google Scholar] [CrossRef] - 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. [Google Scholar] [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. [Google Scholar] [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. [Google Scholar] [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. [Google Scholar] [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. [Google Scholar] [CrossRef] [PubMed] - Han, S.; Wang, J. Integrated GPS/INS navigation system with dual-rate Kalman Filter. GPS Solut.
**2012**, 16, 389–404. [Google Scholar] [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. [Google Scholar] [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. [Google Scholar] [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. [Google Scholar] [CrossRef] - Farrell, J. Aided Navigation: GPS with High Rate Sensors; McGraw-Hill Companies: New York, NY, USA, 2008. [Google Scholar]
- Yang, Y.; Cui, X.; Gao, W. Adaptive integrated navigation for multi-sensor adjustment outputs. J. Navig.
**2004**, 57, 287–295. [Google Scholar] [CrossRef] - Yang, Y.; Gao, W. An optimal adaptive Kalman filter. J. Geod.
**2006**, 80, 177–183. [Google Scholar] [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. [Google Scholar] [CrossRef]

**Figure 3.**The test scenario for verifying the proposed architecture. (

**a**) Test deployment diagram; (

**b**) the locomotive and GPS and the UWB unit.

**Figure 4.**The DOP value variations during the experimental period. (

**a**) In the case of more than four GPS satellite; (

**b**) in the case of four GPS satellite.

**Figure 5.**The residuals of the UWB measurement and its frequency analysis result. (

**a**) Frequency characteristics of the UWB signal; (

**b**) the residuals of the UWB measurement.

**Figure 11.**The trajectories of the locomotive in three GPS blockage periods (Scenario 5). (

**a**) one gap with three satellites; (

**b**) one gap with two satellites; (

**c**) one gap with one satellite.

**Figure 13.**The trajectories with an added GPS gross error using standard Kalman Filter 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 486,820 s.

**Figure 14.**The position accuracy comparison between standard Kalman Filter and RKF with an added pseudo-range gross error.

**Figure 15.**The trajectories with an added UWB gross error using standard Kalman Filter 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 486,820 s.

**Figure 16.**The position accuracy comparison between standard Kalman Filter and RKF with an added UWB ranging gross error.

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 |

Scenarios | Horizontal | Vertical |
---|---|---|

static | 5 mm + 0.5 ppm | 10 mm + 0.5 ppm |

kinematic | 10 mm + 1 ppm | 20 mm + 1 ppm |

Scenario | RMS (m) | MAX (m) | ||||
---|---|---|---|---|---|---|

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 |

Scenario | RMS (m) | MAX (m) | ||||
---|---|---|---|---|---|---|

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 |

Gaps | GPS/INS/UWB (m) | GPS/INS (m) | ||||
---|---|---|---|---|---|---|

North | East | Down | North | East | Down | |

486400s–486450s | 0.42 | 0.17 | 0.39 | 0.56 | 1.34 | 0.39 |

486600s–486650s | 0.41 | 0.18 | 0.53 | 1.03 | 2.65 | 0.54 |

486800s–486850s | 0.46 | 0.17 | 0.58 | 0.54 | 0.92 | 0.58 |

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

Epochs | Standard KF (m) | RKF (m) | ||||
---|---|---|---|---|---|---|

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 |

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

Epochs | Standard KF (m) | RKF (m) | ||||
---|---|---|---|---|---|---|

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 |

© 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/).

## Share and Cite

**MDPI and ACS Style**

Wang, J.; Gao, Y.; Li, Z.; Meng, X.; Hancock, C.M. A Tightly-Coupled GPS/INS/UWB Cooperative Positioning Sensors System Supported by V2I Communication. *Sensors* **2016**, *16*, 944.
https://doi.org/10.3390/s16070944

**AMA Style**

Wang J, Gao Y, Li Z, Meng X, Hancock CM. A Tightly-Coupled GPS/INS/UWB Cooperative Positioning Sensors System Supported by V2I Communication. *Sensors*. 2016; 16(7):944.
https://doi.org/10.3390/s16070944

**Chicago/Turabian Style**

Wang, Jian, Yang Gao, Zengke Li, Xiaolin Meng, and Craig M. Hancock. 2016. "A Tightly-Coupled GPS/INS/UWB Cooperative Positioning Sensors System Supported by V2I Communication" *Sensors* 16, no. 7: 944.
https://doi.org/10.3390/s16070944