OKPS: A Reactive/Cooperative Multi-Sensors Data Fusion Approach Designed for Robust Vehicle Localization
Affiliation(s)
1 Department COSYS, IFSTTAR, LIVIC, Versailles, France. 2 IEF/UMR CNRS 8622, Université Paris-Sud, Centre d’Orsay, Orsay, France.
1 Department COSYS, IFSTTAR, LIVIC, Versailles, France. 2 IEF/UMR CNRS 8622, Université Paris-Sud, Centre d’Orsay, Orsay, France.
ABSTRACT
This paper presents the Optimized Kalman Particle Swarm (OKPS) filter. This filter results from two years of research and improves the Swarm Particle Filter (SPF). The OKPS has been designed to be both cooperative and reactive. It combines the advantages of the Particle Filter (PF) and the metaheuristic Particle Swarm Optimization (PSO) for ego-vehicles localization applications. In addition to a simple fusion between the swarm optimization and the particular filtering (which leads to the Swarm Particle Filter), the OKPS uses some attributes of the Extended Kalman filter (EKF). The OKPS filter innovates by fitting its particles with a capacity of self-diagnose by means of the EKF covariance uncertainty matrix. The particles can therefore evolve by exchanging information to assess the optimized position of the ego-vehicle. The OKPS fuses data coming from embedded sensors (low cost INS, GPS and Odometer) to perform a robust ego-vehicle positioning. The OKPS is compared to the EKF filter and to filters using particles (PF and SPF) on real data from our equipped vehicle.
This paper presents the Optimized Kalman Particle Swarm (OKPS) filter. This filter results from two years of research and improves the Swarm Particle Filter (SPF). The OKPS has been designed to be both cooperative and reactive. It combines the advantages of the Particle Filter (PF) and the metaheuristic Particle Swarm Optimization (PSO) for ego-vehicles localization applications. In addition to a simple fusion between the swarm optimization and the particular filtering (which leads to the Swarm Particle Filter), the OKPS uses some attributes of the Extended Kalman filter (EKF). The OKPS filter innovates by fitting its particles with a capacity of self-diagnose by means of the EKF covariance uncertainty matrix. The particles can therefore evolve by exchanging information to assess the optimized position of the ego-vehicle. The OKPS fuses data coming from embedded sensors (low cost INS, GPS and Odometer) to perform a robust ego-vehicle positioning. The OKPS is compared to the EKF filter and to filters using particles (PF and SPF) on real data from our equipped vehicle.
KEYWORDS
Localization, Mobile Robotic, Extended Kalman Filter, Particle Swarm Optimization, Particle Filter, Data Fusion, Vehicle Positioning, GPS
Localization, Mobile Robotic, Extended Kalman Filter, Particle Swarm Optimization, Particle Filter, Data Fusion, Vehicle Positioning, GPS
Cite this paper
Bacha, A. , Gruyer, D. and Lambert, A. (2016) OKPS: A Reactive/Cooperative Multi-Sensors Data Fusion Approach Designed for Robust Vehicle Localization. Positioning, 7, 1-20. doi: 10.4236/pos.2016.71001.
Bacha, A. , Gruyer, D. and Lambert, A. (2016) OKPS: A Reactive/Cooperative Multi-Sensors Data Fusion Approach Designed for Robust Vehicle Localization. Positioning, 7, 1-20. doi: 10.4236/pos.2016.71001.
References
[1] Bouaziz, S., Fan, M., Lambert, A., Maurin, T. and Reynaud, R. (2003) PICAR: Experimental Platform for Road Tracking Applications. IEEE Intelligent Vehicles Symposium, Columbus, 9-11 June 2003, 495-499.
http://dx.doi.org/10.1109/IVS.2003.1212961 [2] Pepy, R., Lambert, A. and Mounier, H. (2006) Reducing Navigation Errors by Planning with Realistic Vehicle Model. 2006 IEEE Intelligent Vehicles Symposium, Tokyo, 13-15 June 2006, 300-307.
http://dx.doi.org/10.1109/ivs.2006.1689645 [3] Seignez, E., Lambert, A. and Maurin, T. (2005) Autonomous Parking Carrier for Intelligent Vehicle. IEEE International Conference on Intelligent Vehicle, Las Vegas, 6-8 June 2005, 411-416.
http://dx.doi.org/10.1109/ivs.2005.1505138 [4] Ndjeng, A.N., Lambert, A., Gruyer, D. and Glaser, S. (2009) Experimental Comparison of Kalman Filters for Vehicle Localization. IEEE Intelligent Vehicles Symposium, Xi’an, 3-5 June 2009, 441-446.
http://dx.doi.org/10.1109/ivs.2009.5164318 [5] Maybeck, P.S. (1982) Stochastic Models, Estimation and Control. Academic Press, New York. [6] Lewis, F.L. (1986) Optimal Estimation: With an Introduction to Stochastic Control Theory. John Wiley & Sons, New York. [7] Lambert, A., Gruyer, D., Vincke, B. and Seignez, E. (2009) Consistent Outdoor Vehicle Localization by Bounded-Er- ror State Estimation. IEEE/RSJ International Conference on Intelligent Robots and Systems, St. Louis, 10-15 October 2009, 1211-1216. http://dx.doi.org/10.1109/iros.2009.5354673 [8] Kalman, R.E. (1960) A New Approach to Linear Filtering and Prediction System. Transactions of the ASME—Journal of Basic Engineering, 82, 35-45. http://dx.doi.org/10.1115/1.3662552 [9] Julier, S.J. and Uhlmann, J. (1997) A New Extension of the Kalman Filter to Nonlinear Systems. International Symposium on Aerospace/Defense Sensing, Simulation and Controls, Orlando, 182-193. [10] Ito, K. and Xiong, K. (2000) Gaussian Filters for Nonlinear Filtering Problems. IEEE Transaction on Automatic Control, 45, 910-927. http://dx.doi.org/10.1109/9.855552 [11] Julier, S.J. and Uhlmann, J. (1996) A General Method for Approximating Nonlinear Transformation of Probability Distribution. Technical Report, University of Oxford, Oxford. [12] Lefebvre, T., Bruyninckx, H. and De Schutter, J. (2004) Kalman Filters for Non-Linear Systems: A Comparison of Performance. International Journal of Control, 77, 639-653.
http://dx.doi.org/10.1080/00207170410001704998 [13] Mourllion, B., Gruyer, D., Lambert, A. and Glaser, S. (2005) Kalman Filters Comparison for Vehicle Localization Data Alignment. IEEE/RSJ International Conference on Advanced Robotics, Seattle, 18-20 July 2005, 178-185. http://dx.doi.org/10.1109/icar.2005.1507410 [14] Ali, J. and Ullah Baig Mirza, M.R. (2010) Performance Comparison among Some Nonlinear Filters for a Low Cost SINS/GPS Integrated Solution. Nonlinear Dynamics, 61, 491-502.
http://dx.doi.org/10.1007/s11071-010-9665-y [15] Kandepu, R., Foss, B. and Imsland, L. (2008) Applying the Unscented Kalman Filter for Nonlinear State Estimation. Journal of Process Control, 18, 753-768. http://dx.doi.org/10.1016/j.jprocont.2007.11.004 [16] Havangi, R., Nekoui, M.A. and Teshnehlab, M. (2010) A Multi Swarm Particle Filter for Mobile Robot Localization. International Journal of Computer Science, 7, 15-22. [17] Godoy, J., Gruyer, D., Lambert, A. and Villagra, J. (2012) Development of an Particle Swarm Algorithm for Vehicle Localization. IEEE Intelligent Vehicles Symposium, Alcala de Henares, 3-7 June 2012, 1114-1119. http://dx.doi.org/10.1109/ivs.2012.6232213 [18] Bazzani, L., Bloisi, D. and Murino, V. (2009) A Comparison of Multi Hypothesis Kalman Filter and Particle Filter for Multi-Target Tracking. Performance Evaluation of Tracking and Surveillance Workshop at CVPR, Miami, 25 June 2009, 47-54. [19] Arulampalam, S., Maskell, S., Gordon, N. and Clapp, T. (2002) A Tutorial on Particle Filters for Online Non-Linear/ Non-Gaussian Bayesian Tracking. IEEE Transactions on Signal Processing, 50, 174-188.
http://dx.doi.org/10.1109/78.978374 [20] Hol, J.D., Schön, T.B. and Gustafsson, F. (2006) On Resampling Algorithms for Particle Filters. 2006 IEEE Nonlinear Statistical Signal Processing Workshop, Cambridge, 13-15 September 2006, 79-82.
http://dx.doi.org/10.1109/nsspw.2006.4378824 [21] Shi, Y. and Eberhart, R. (1998) A Modified Particle Swarm Optimizer. IEEE International Conference on Evolutionary Computation, Anchorage, 4-9 May 1998, 69-73.
http://dx.doi.org/10.1109/icec.1998.699146 [22] Reyes-Sierra, M. and Coello, C.A.C. (2006) Multi-Objective Particle Swarm Optimizers: A Survey of the State-of- the-Art. International Journal of Computational Intelligence Research, 2, 287-308. [23] Coello Coello, C.A. and Lechuga, M.S. (2002) MOPSO: A Proposal for Multiple Objective Particle Swarm Optimization. IEEE Congress on Evolutionary Computation, 2, 1051-1056.
http://dx.doi.org/10.1109/cec.2002.1004388 [24] Banks, A., Vincent, J. and Anyakoha, C. (2007) A Review of Particle Swarm Optimization. Part I: Background and Development. Natural Computing, 6, 467-484.
http://dx.doi.org/10.1007/s11047-007-9049-5 [25] Esquivel, S.C. and Coello, C.A.C. (2003) On the Use of Particle Swarm Optimization with Multimodal Functions. IEEE Congress on Evolutionary Computation, Canberra, 8-12 December 2003, 1130-1136.
http://dx.doi.org/10.1109/cec.2003.1299795 [26] Jwo, D.-J. and Chang, S.-C. (2009) Particle Swarm Optimization for GPS Navigation Kalman Filter Adaptation. Aircraft Engineering and Aerospace Technology, 81, 343-352.
http://dx.doi.org/10.1108/00022660910967336 [27] Zhang, J., Pan, T.-S. and Pan, J.-S. (2011) A Parallel Hybrid Evolutionary Particle Filter for Nonlinear State Estimation. 2011 1st International Conference on Robot, Vision and Signal Processing (RVSP), Kaohsiung, 21-23 November 2011, 308-312. http://dx.doi.org/10.1109/RVSP.2011.77 [28] Tong, G., Fang, Z. and Xu, X. (2006) A Particle Swarm Optimized Particle Filter for Nonlinear System State Estimation. IEEE Congress on Evolutionary Computation, CEC 2006, Vancouver, 16-21 July 2006, 438-442. http://dx.doi.org/10.1109/CEC.2006.1688342 [29] Fang, Z., Tong, G.-F. and Xu, X.-H. (2007) Particle Swarm Optimized Particle Filter. Control and Decision, 22, 273-277. [30] Chen, Z.-M., Bo, Y.-M., Wu, P.-L. and Chen, Q.-X. (2012) A New Hybrid Algorithm for Particle Filtering and Its Application to Radar Target Tracking. Acta Armamentarii, 33, 83-88. [31] Ahmed Bacha, A., Gruyer, D. and Lambert, A. (2013) A Robust Hybrid Multisource Data Fusion Approach for Vehicle Localization. Positioning, 4, 271-281. http://dx.doi.org/10.4236/pos.2013.44027 [32] Ahmed Bacha, A., Gruyer, D. and Lambert, A. (2014) A Performance Test for a New Reactive-Cooperative Filter in an Ego-Vehicle Localization Application. IV 2014 IEEE Intelligent Vehicles Symposium (IV), Dearborn, 8-11 June 2014, 548-554. http://dx.doi.org/10.1109/IVS.2014.6856472 [33] Clerc, M. and Kennedy, J. (2002) The Particle Swarm—Explosion, Stability, and Convergence in a Multidimensional Complex Space. IEEE Transactions on Evolutionary Computation, 6, 58-73.
http://dx.doi.org/10.1109/4235.985692 [34] Krohling, R.A. (2004) Gaussian Swarm: A Novel Particle Swarm Optimization Algorithm. IEEE Conference on Cybernetics and Intelligent Systems, 1, 372-376. [35] Hu, X.H., Shi, Y.H. and Eberhart, R. (2004) Recent Advances in Particle Swarm. 2004 Congress on Evolutionary Computation, 1, 90-97. [36] Frans, V.D.B. (2002) An Analysis of Particle Swarm Optimizers. PhD Thesis, University of Pretoria, Pretoria. [37] Liang, X., Li, W., Zhang, Y., Zhong, Y. and Zhou, M. (2013) Recent Advances in Particle Swarm Optimization via Population Structuring and Individual Behavior Control. 2013 10th IEEE International Conference on Networking, Sensing and Control (ICNSC), Evry, 10-12 April 2013, 503-508.
http://dx.doi.org/10.1109/ICNSC.2013.6548790
[1] Bouaziz, S., Fan, M., Lambert, A., Maurin, T. and Reynaud, R. (2003) PICAR: Experimental Platform for Road Tracking Applications. IEEE Intelligent Vehicles Symposium, Columbus, 9-11 June 2003, 495-499.
http://dx.doi.org/10.1109/IVS.2003.1212961 [2] Pepy, R., Lambert, A. and Mounier, H. (2006) Reducing Navigation Errors by Planning with Realistic Vehicle Model. 2006 IEEE Intelligent Vehicles Symposium, Tokyo, 13-15 June 2006, 300-307.
http://dx.doi.org/10.1109/ivs.2006.1689645 [3] Seignez, E., Lambert, A. and Maurin, T. (2005) Autonomous Parking Carrier for Intelligent Vehicle. IEEE International Conference on Intelligent Vehicle, Las Vegas, 6-8 June 2005, 411-416.
http://dx.doi.org/10.1109/ivs.2005.1505138 [4] Ndjeng, A.N., Lambert, A., Gruyer, D. and Glaser, S. (2009) Experimental Comparison of Kalman Filters for Vehicle Localization. IEEE Intelligent Vehicles Symposium, Xi’an, 3-5 June 2009, 441-446.
http://dx.doi.org/10.1109/ivs.2009.5164318 [5] Maybeck, P.S. (1982) Stochastic Models, Estimation and Control. Academic Press, New York. [6] Lewis, F.L. (1986) Optimal Estimation: With an Introduction to Stochastic Control Theory. John Wiley & Sons, New York. [7] Lambert, A., Gruyer, D., Vincke, B. and Seignez, E. (2009) Consistent Outdoor Vehicle Localization by Bounded-Er- ror State Estimation. IEEE/RSJ International Conference on Intelligent Robots and Systems, St. Louis, 10-15 October 2009, 1211-1216. http://dx.doi.org/10.1109/iros.2009.5354673 [8] Kalman, R.E. (1960) A New Approach to Linear Filtering and Prediction System. Transactions of the ASME—Journal of Basic Engineering, 82, 35-45. http://dx.doi.org/10.1115/1.3662552 [9] Julier, S.J. and Uhlmann, J. (1997) A New Extension of the Kalman Filter to Nonlinear Systems. International Symposium on Aerospace/Defense Sensing, Simulation and Controls, Orlando, 182-193. [10] Ito, K. and Xiong, K. (2000) Gaussian Filters for Nonlinear Filtering Problems. IEEE Transaction on Automatic Control, 45, 910-927. http://dx.doi.org/10.1109/9.855552 [11] Julier, S.J. and Uhlmann, J. (1996) A General Method for Approximating Nonlinear Transformation of Probability Distribution. Technical Report, University of Oxford, Oxford. [12] Lefebvre, T., Bruyninckx, H. and De Schutter, J. (2004) Kalman Filters for Non-Linear Systems: A Comparison of Performance. International Journal of Control, 77, 639-653.
http://dx.doi.org/10.1080/00207170410001704998 [13] Mourllion, B., Gruyer, D., Lambert, A. and Glaser, S. (2005) Kalman Filters Comparison for Vehicle Localization Data Alignment. IEEE/RSJ International Conference on Advanced Robotics, Seattle, 18-20 July 2005, 178-185. http://dx.doi.org/10.1109/icar.2005.1507410 [14] Ali, J. and Ullah Baig Mirza, M.R. (2010) Performance Comparison among Some Nonlinear Filters for a Low Cost SINS/GPS Integrated Solution. Nonlinear Dynamics, 61, 491-502.
http://dx.doi.org/10.1007/s11071-010-9665-y [15] Kandepu, R., Foss, B. and Imsland, L. (2008) Applying the Unscented Kalman Filter for Nonlinear State Estimation. Journal of Process Control, 18, 753-768. http://dx.doi.org/10.1016/j.jprocont.2007.11.004 [16] Havangi, R., Nekoui, M.A. and Teshnehlab, M. (2010) A Multi Swarm Particle Filter for Mobile Robot Localization. International Journal of Computer Science, 7, 15-22. [17] Godoy, J., Gruyer, D., Lambert, A. and Villagra, J. (2012) Development of an Particle Swarm Algorithm for Vehicle Localization. IEEE Intelligent Vehicles Symposium, Alcala de Henares, 3-7 June 2012, 1114-1119. http://dx.doi.org/10.1109/ivs.2012.6232213 [18] Bazzani, L., Bloisi, D. and Murino, V. (2009) A Comparison of Multi Hypothesis Kalman Filter and Particle Filter for Multi-Target Tracking. Performance Evaluation of Tracking and Surveillance Workshop at CVPR, Miami, 25 June 2009, 47-54. [19] Arulampalam, S., Maskell, S., Gordon, N. and Clapp, T. (2002) A Tutorial on Particle Filters for Online Non-Linear/ Non-Gaussian Bayesian Tracking. IEEE Transactions on Signal Processing, 50, 174-188.
http://dx.doi.org/10.1109/78.978374 [20] Hol, J.D., Schön, T.B. and Gustafsson, F. (2006) On Resampling Algorithms for Particle Filters. 2006 IEEE Nonlinear Statistical Signal Processing Workshop, Cambridge, 13-15 September 2006, 79-82.
http://dx.doi.org/10.1109/nsspw.2006.4378824 [21] Shi, Y. and Eberhart, R. (1998) A Modified Particle Swarm Optimizer. IEEE International Conference on Evolutionary Computation, Anchorage, 4-9 May 1998, 69-73.
http://dx.doi.org/10.1109/icec.1998.699146 [22] Reyes-Sierra, M. and Coello, C.A.C. (2006) Multi-Objective Particle Swarm Optimizers: A Survey of the State-of- the-Art. International Journal of Computational Intelligence Research, 2, 287-308. [23] Coello Coello, C.A. and Lechuga, M.S. (2002) MOPSO: A Proposal for Multiple Objective Particle Swarm Optimization. IEEE Congress on Evolutionary Computation, 2, 1051-1056.
http://dx.doi.org/10.1109/cec.2002.1004388 [24] Banks, A., Vincent, J. and Anyakoha, C. (2007) A Review of Particle Swarm Optimization. Part I: Background and Development. Natural Computing, 6, 467-484.
http://dx.doi.org/10.1007/s11047-007-9049-5 [25] Esquivel, S.C. and Coello, C.A.C. (2003) On the Use of Particle Swarm Optimization with Multimodal Functions. IEEE Congress on Evolutionary Computation, Canberra, 8-12 December 2003, 1130-1136.
http://dx.doi.org/10.1109/cec.2003.1299795 [26] Jwo, D.-J. and Chang, S.-C. (2009) Particle Swarm Optimization for GPS Navigation Kalman Filter Adaptation. Aircraft Engineering and Aerospace Technology, 81, 343-352.
http://dx.doi.org/10.1108/00022660910967336 [27] Zhang, J., Pan, T.-S. and Pan, J.-S. (2011) A Parallel Hybrid Evolutionary Particle Filter for Nonlinear State Estimation. 2011 1st International Conference on Robot, Vision and Signal Processing (RVSP), Kaohsiung, 21-23 November 2011, 308-312. http://dx.doi.org/10.1109/RVSP.2011.77 [28] Tong, G., Fang, Z. and Xu, X. (2006) A Particle Swarm Optimized Particle Filter for Nonlinear System State Estimation. IEEE Congress on Evolutionary Computation, CEC 2006, Vancouver, 16-21 July 2006, 438-442. http://dx.doi.org/10.1109/CEC.2006.1688342 [29] Fang, Z., Tong, G.-F. and Xu, X.-H. (2007) Particle Swarm Optimized Particle Filter. Control and Decision, 22, 273-277. [30] Chen, Z.-M., Bo, Y.-M., Wu, P.-L. and Chen, Q.-X. (2012) A New Hybrid Algorithm for Particle Filtering and Its Application to Radar Target Tracking. Acta Armamentarii, 33, 83-88. [31] Ahmed Bacha, A., Gruyer, D. and Lambert, A. (2013) A Robust Hybrid Multisource Data Fusion Approach for Vehicle Localization. Positioning, 4, 271-281. http://dx.doi.org/10.4236/pos.2013.44027 [32] Ahmed Bacha, A., Gruyer, D. and Lambert, A. (2014) A Performance Test for a New Reactive-Cooperative Filter in an Ego-Vehicle Localization Application. IV 2014 IEEE Intelligent Vehicles Symposium (IV), Dearborn, 8-11 June 2014, 548-554. http://dx.doi.org/10.1109/IVS.2014.6856472 [33] Clerc, M. and Kennedy, J. (2002) The Particle Swarm—Explosion, Stability, and Convergence in a Multidimensional Complex Space. IEEE Transactions on Evolutionary Computation, 6, 58-73.
http://dx.doi.org/10.1109/4235.985692 [34] Krohling, R.A. (2004) Gaussian Swarm: A Novel Particle Swarm Optimization Algorithm. IEEE Conference on Cybernetics and Intelligent Systems, 1, 372-376. [35] Hu, X.H., Shi, Y.H. and Eberhart, R. (2004) Recent Advances in Particle Swarm. 2004 Congress on Evolutionary Computation, 1, 90-97. [36] Frans, V.D.B. (2002) An Analysis of Particle Swarm Optimizers. PhD Thesis, University of Pretoria, Pretoria. [37] Liang, X., Li, W., Zhang, Y., Zhong, Y. and Zhou, M. (2013) Recent Advances in Particle Swarm Optimization via Population Structuring and Individual Behavior Control. 2013 10th IEEE International Conference on Networking, Sensing and Control (ICNSC), Evry, 10-12 April 2013, 503-508.
http://dx.doi.org/10.1109/ICNSC.2013.6548790