A Multiple-Model Particle Filter Fusion Algorithm for GNSS/DR Slide Error Detection and Compensation
Abstract
:1. Introduction
2. Related Work Materials and Methods
3. Error Model of the Odometry
4. Multiple Model Particle Filter Based Method
- Initialization: Generation of N particles, or samples of the state vector, Xi(0), with equal weights 1/N. The proposed state vector is , representing east, north and heading (from north to east) at the center of the rear axle of the vehicle.
- Prediction: Estimation of Xi(k + 1) following the prediction model. We use a classical 2D kinematical model for a vehicle on a plane. The measurements of the odometry and the gyroscope work as inputs to the filter. The travelled distance measured by the odometer, ds(k), is estimated when a gyroscope value, , is processed. The equation for pose prediction is:
- Measurement update: Update of the weights of the particles with the observations Y(k). In our case, the observation vector is , standing for east and north values coming from the GPS. The update is done following the expression
- Normalization of the weights: .
- Resampling: To prevent high concentration of probability mass in only a few particles, (leading to the convergence of a single wi(k) to 1), particles are resampled if
- End of cycle: Making k = k + 1, and iterating to step 2.
5. Filter Consistency
5.1. Filter Covariance
5.2. Time Average Autocorrelation
5.3. Time Average Normalized Innovation Square
6. Tests
6.1. Test Setup
- EGNOS capable GPS receiver by Trimble, L1.
- Dual-frequency RTK receiver by Ashtech, for ground reference.
- FOG (Fiber Optic Gyroscope) by KVH.
- Vehicle odometer with 26.15 cm resolution coupled to the rear wheels axle.
6.2. Results and Discussion
7. Conclusions
Acknowledgments
Author Contributions
Conflicts of Interest
References
- Bétaille, D.; Bonnifait, P.; Blanco Delgado, N.; Cosmen Schortmann, J.; Engdahl, J.; Gikas, V.; Gilliéron, P.Y.; Hodon, M.; Feng, S.; Machaj, J.; et al. SaPPART White Paper. Better Use of Global Navigation Satellite Systems for Safer and Greener Transport; IFSTTAR Techniques et Méthodes; IFSTTAR: Paris, France, 2015. [Google Scholar]
- Toledo, R.; Zamora, M.A.; Ubeda, B.; Gomez-Skarmeta, A.F. An Integrity Navigation System based on GNSS/INS for Remote Services Implementation in Terrestrial Vehicles. In Proceedings of the 7th International IEEE Conference on Intelligent Transportation Systems, Washington, DC, USA, 3–6 October 2004; pp. 477–480. [Google Scholar]
- Toledo-Moreo, R.; Bétaille, D.; Peyret, F. Lane-Level Integrity Provision for Navigation and Map Matching With GNSS, Dead Reckoning, and Enhanced Maps. IEEE Trans. Intell. Transp. Syst. 2010, 11, 100–112. [Google Scholar] [CrossRef]
- Sukkarieh, S.; Durant-Whyte, H.; Nebot, E. A High Integrity IMU/GPS Navigation Loop for Autonomous Land Vehicle Applications. IEEE Trans. Robot. Autom. 1999, 15, 572–578. [Google Scholar] [CrossRef]
- Wilson, C.; Wang, J. Safety at the Wheel. Improving KGPS/INS Performance and Reliability; GPS World: Duluth, MN, USA, 2003; pp. 16–26. [Google Scholar]
- Berdjag, D.; Pomorski, D. DGPS/INS data fusion for land navigation. In Proceedings of the Fusion 2004, Stockholm, Sweden, 28 June–1 July 2004. [Google Scholar]
- Zhang, P.; Gu, J.; Milios, E.E.; Huynh, P. Navigation with IMU/GPS/digital compass with unscented Kalman filter. In Proceedings of the IEEE International Conference on Mechatronics and Automation, Niagara Falls, ON, Canada, 29 July–1 August 2005; pp. 1497–1502. [Google Scholar]
- Liu, B.; Adam, M.; Ibañez-Guzman, J. Multi-aided Inertial Navigation for Ground Vehicles in Outdoor Uneven Environments. In Proceedings of the 2005 IEEE International Conference on Robotics and Automation, Barcelona, Spain, 18–22 April 2005. [Google Scholar]
- Toledo-Moreo, R.; Zamora-Izquierdo, M.; Ubeda, B.; Skarmeta, M.A. High-Integrity IMM-EKF-Based Road Vehicle. IEEE Trans. Intell. Transp. Syst. 2007, 8, 491–511. [Google Scholar] [CrossRef]
- Takeyama, K.; Machida, T.; Kojima, Y.; Kubo, N. Improvement of Dead Reckoning in Urban Areas through Integration of Low-Cost Multisensors. IEEE Trans. Intell. Veh. 2017, 2, 278–287. [Google Scholar] [CrossRef]
- Alonso, I.P.; Llorca, D.F.; Gavilan, M.; Pardo, S.Á.; Garcia-Garrido, M.A.; Vlacic, L.; Sotelo, M.Á. Accurate Global Localization Using Visual Odometry and Digital Maps on Urban Environments. IEEE Trans. Intell. Transp. Syst. 2012, 13, 1535–1545. [Google Scholar] [CrossRef]
- Kunii, Y.; Kovacs, G.; Hoshi, N. Mobile robot navigation in natural environments using robust object tracking. In Proceedings of the IEEE 26th International Symposium on Industrial Electronics (ISIE), Edinburgh, UK, 19–21 June 2017; pp. 1747–1752. [Google Scholar]
- Chaves, G.D.L.; Martins, L.G.A.; de Oliveira, G.M.B. An improved model based on cellular automata for on-line navigation. In Proceedings of the 2017 Latin American Robotics Symposium (LARS) and 2017 Brazilian Symposium on Robotics (SBR), Curitiba, Brazil, 8–11 November 2017. [Google Scholar]
- Bar-Shalom, Y. Estimation and Tracking: Principles; Artech House: Nonvood, MA, USA, 1993. [Google Scholar]
- Skog, I.; Handel, P. In-Car Positioning and Navigation Technologies—A Survey. IEEE Trans. Intell. Transp. Syst. 2009, 10, 4–21. [Google Scholar] [CrossRef]
- Doucet, A.; de Freitas, N.; Gordon, N. Sequential Monte Carlo; Springer: New York, NY, USA, 2001. [Google Scholar]
- Djuric, P.M.; Kotecha, J.H.; Zhang, J.; Huang, Y.; Ghirmai, T.; Bugallo, M.F.; Miguez, J. Particle filtering. IEEE Signal Proceess. Mag. 2003, 20, 19–38. [Google Scholar] [CrossRef]
- Toledo-Moreo, R.; Zamora-Izquierdo, M. IMM-Based Lane-Change Prediction in Highways with Low-Cost GPS/INS. IEEE Trans. Intell. Transp. Syst. 2009, 10, 180–185. [Google Scholar] [CrossRef]
- Ekman, M.; Sviestins, E. Multiple Model Algorithm Based on Particle Filters for Ground Target Tracking. In Proceedings of the 2007 10th International Conference on Information Fusion, Quebec, QC, Canada, 9–12 July 2007; pp. 1–8. [Google Scholar]
- Hong, L.; Cui, N.; Bakich, M.; Layne, J.R. Multirate interacting multiple model particle filter for terrain-based ground target tracking. IEE Proc. Control Theory Appl. 2006, 53, 721–731. [Google Scholar] [CrossRef]
- Guo, R.; Qin, Z.; Li, X.; Chen, J. IMMUPF Method for Ground Target Tracking. In Proceedings of the IEEE International Conference on Systems, Man and Cybernetics, Montreal, QC, Canada, 7–10 October 2007; pp. 96–101. [Google Scholar]
- Foo, P.H.; Ng, G.W. Combining IMM Method with Particle Filters for 3D Maneuvering Target Tracking. In Proceedings of the 2007 10th International Conference on Information Fusion, Quebec, QC, Canada, 9–12 July 2007; pp. 1–8. [Google Scholar]
- Wang, J.; Zhao, D.; Gao, W.; Shan, S. Interacting Multiple Model Particle Filter to Adaptive Visual Tracking. In Proceedings of the Image and Graphics (ICIG’04), Hong Kong, China, 18–20 December 2004; pp. 568–571. [Google Scholar]
- Giremus, A.; Tourneret, J.-Y.; Calmettes, V. A Particle Filtering Approach for Joint Detection/Estimation of Multipath Effects on GPS Measurements. IEEE Trans. Signal Process. 2007, 55, 1275–1285. [Google Scholar] [CrossRef] [Green Version]
Model | Value |
---|---|
NCMPF | 0.5762 |
SCMPF | 0.9110 |
MMPF | 0.0140 |
© 2018 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
Toledo-Moreo, R.; Colodro-Conde, C.; Toledo-Moreo, J. A Multiple-Model Particle Filter Fusion Algorithm for GNSS/DR Slide Error Detection and Compensation. Appl. Sci. 2018, 8, 445. https://doi.org/10.3390/app8030445
Toledo-Moreo R, Colodro-Conde C, Toledo-Moreo J. A Multiple-Model Particle Filter Fusion Algorithm for GNSS/DR Slide Error Detection and Compensation. Applied Sciences. 2018; 8(3):445. https://doi.org/10.3390/app8030445
Chicago/Turabian StyleToledo-Moreo, Rafael, Carlos Colodro-Conde, and Javier Toledo-Moreo. 2018. "A Multiple-Model Particle Filter Fusion Algorithm for GNSS/DR Slide Error Detection and Compensation" Applied Sciences 8, no. 3: 445. https://doi.org/10.3390/app8030445