2. Problem Description
- Each vehicle has an odometry and a Global Positioning System (GPS) sensor to localize itself in an absolute reference and can broadcasts its measurements.
- The infrastructure sensor that can derive the global position of all the vehicles in its field of view, but cannot uniquely identify the vehicles. This introduces a challenge from the perspective of data association. A typical example of such sensor is a RADAR. RADAR has been extensively used by the military for surveillance . Since the mid 1990s, it has been researched as an active  or passive  component of the Intelligent Vehicle Highway System (IVHS). Recently however, because of lowering of costs, it has gained a lot of traction as an infrastructure sensor for smart highways [24,25,26,27].
- The vehicles and the infrastructure sensor can communicate in both directions without any timing delay or data error.
- The environment has no clutter and there are no missed detections.
3. Symmetric Measurement Equations (SMEs)
4. Non-Linear Least Square Optimization
4.1. Factor Graphs
- from the odometry sensor,
- from the generic sensor 1,
- from the generic sensor 2,
4.2. Odometry Factor
4.3. GPS Factor
4.4. SME Factor
- The odometry and GPS measurements from all/other vehicles;
- Absolute positions, in global coordinates, of all vehicles in the field of view of a configured RADAR. Here by configuration of RADAR we imply that it knows its position in global coordinates and hence is able to perform a coordinate transformation of the measurements of the detected targets in its local coordinates to the global coordinates.
5.1. Simulation Setup
- Two vehicles with random trajectories on a ground plane with an infrastructure RADAR.
- Three vehicles with a constant turn model on a ground plane with an infrastructure RADAR.
- An intersection with five vehicles with an infrastructure RADAR mounted at the center of intersection (Figure 3). We assume the infrastructure RADAR has an equal field of view for all the four directions. This test uses the trajectories with a constant velocity model.
- Monte Carlo simulations for 1000 iterations for 2, 3 and 4 vehicles. This test also reflects the trajectories with a constant velocity model.
- the fused trajectory only using odometry;
- the fused trajectory for odometry and topology factor ; and
- the fused trajectory for the odometry and SME factor (proposed in this work).
5.3. Plug and Play and Online Execution
5.4. Final Remarks
Conflicts of Interest
- Kurazume, R.; Nagata, S.; Hirose, S. Cooperative positioning with multiple robots. In Proceedings of the 1994 IEEE International Conference on Robotics and Automation, San Diego, CA, USA, 8–13 May 1994; Volume 2, pp. 1250–1257. [Google Scholar]
- Roumeliotis, S.I.; Bekey, G.A. Distributed multirobot localization. IEEE Trans. Robot. Autom. 2002, 18, 781–795. [Google Scholar] [CrossRef]
- Karam, N.; Chausse, F.; Aufrere, R.; Chapuis, R. Cooperative multi-vehicle localization. In Proceedings of the 2006 IEEE Intelligent Vehicles Symposium, Tokyo, Japan, 13–15 June 2006; pp. 564–570. [Google Scholar]
- Nerurkar, E.D.; Roumeliotis, S.I.; Martinelli, A. Distributed maximum a posteriori estimation for multi-robot cooperative localization. In Proceedings of the IEEE International Conference on Robotics and Automation, Kobe, Japan, 12–17 May 2009; pp. 1402–1409. [Google Scholar]
- Rekleitis, I.M.; Dudek, G.; Milios, E.E. Multi-robot cooperative localization: A study of trade-offs between efficiency and accuracy. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Lausanne, Switzerland, 30 September–4 October 2002; Volume 3, pp. 2690–2695. [Google Scholar]
- Fox, D.; Burgard, W.; Kruppa, H.; Thrun, S. A probabilistic approach to collaborative multi-robot localization. Auton. Robot. 2000, 8, 325–344. [Google Scholar] [CrossRef]
- Li, H.; Nashashibi, F. Cooperative Multi-Vehicle Localization Using Split Covariance Intersection Filter. IEEE Intell. Transp. Syst. Mag. 2013, 5, 33–44. [Google Scholar] [CrossRef]
- Zhang, F.; Stähle, H.; Chen, G.; Buckl, C.; Knoll, A. Multiple vehicle cooperative localization under random finite set framework. In Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, Japan, 3–7 November 2013; pp. 1405–1411. [Google Scholar]
- Howard, A.; Matark, M.J.; Sukhatme, G.S. Localization for mobile robot teams using maximum likelihood estimation. In Proceedings of the 2002 IEEE/RSJ International Conference on Intelligent Robots and Systems, Lausanne, Switzerland, 30 September–4 October 2002; Volume 1, pp. 434–439. [Google Scholar]
- Ahmad, A.; Tipaldi, G.D.; Lima, P.; Burgard, W. Cooperative robot localization and target tracking based on least squares minimization. In Proceedings of the 2013 IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, 6–10 May 2013; pp. 5696–5701. [Google Scholar]
- Gulati, D.; Zhang, F.; Clarke, D.; Knoll, A. Vehicle infrastructure cooperative localization using Factor Graphs. In Proceedings of the 2016 IEEE Intelligent Vehicles Symposium (IV), Gothenburg, Sweden, 19–22 June 2016; pp. 1085–1090. [Google Scholar]
- GTSAM, Georgia Tech Smoothing and Mapping. Available online: https://collab.cc.gatech.edu/borg/gtsam/ (accessed on 16 June 2017).
- Fortmann, T.E.; Bar-Shalom, Y.; Scheffe, M. Multi-target tracking using joint probabilistic data association. In Proceedings of the 1980 19th IEEE Conference on Decision and Control including the Symposium on Adaptive Processes, Albuquerque, NM, USA, 10–12 December 1980; pp. 807–812. [Google Scholar]
- Rezatofighi, S.H.; Milan, A.; Zhang, Z.; Shi, Q.; Dick, A.; Reid, I. Joint Probabilistic Data Association Revisited. In Proceedings of the 2015 IEEE International Conference on Computer Vision, Santiago, Chile, 13–16 December 2015; pp. 3047–3055. [Google Scholar]
- Reid, D. An algorithm for tracking multiple targets. IEEE Trans. Autom. Control 1979, 24, 843–854. [Google Scholar] [CrossRef]
- Streit, R.L.; Luginbuhl, T.E. A probabilistic multi-hypothesis tracking algorithm without enumeration and pruning. In Proceedings of the Sixth Joint Service Data Fusion Symposium, Maryland, USA, 14–18 June 1993; pp. 1015–1024. [Google Scholar]
- Giannopoulos, E.; Streit, R.; Swaszek, P. Probabilistic multi-hypothesis tracking in a multi-sensor, multi-target environment. In Proceedings of the First Australian Data Fusion Symposium, Adelaide, Australia, 21–22 November 1996; pp. 184–189. [Google Scholar]
- Mahler, R.P.S. Multitarget Bayes filtering via first-order multitarget moments. IEEE Trans. Aerosp. Electron. Syst. 2003, 39, 1152–1178. [Google Scholar] [CrossRef]
- Kamen, E.W. Multiple target tracking based on symmetric measurement equations. IEEE Trans. Autom. Control 1992, 37, 371–374. [Google Scholar] [CrossRef]
- Zhang, F.; Hinz, G.; Gulati, D.; Clarke, D.; Knoll, A. Cooperative Vehicle-Infrastructure Localization Based on the Symmetric Measurement Equation Filter. Geoinformatica 2016, 20, 159–178. [Google Scholar] [CrossRef]
- Skolnik, M.I. Introduction to Radar Systems; Mc Graw-Hill: New York, NY, USA, 2001. [Google Scholar]
- Higgins, J. Doppler Radar System for Automotive Vehicles. U.S. Patent 5,481,268, 2 January 1996. [Google Scholar]
- Akutsu, E. Vehicle Position Detection System. U.S. Patent 6,081,187, 27 June 2000. [Google Scholar]
- Digital Test Field. Available online: www.siemens.com/presse/digitales-testfeld-a9 (accessed on 16 June 2017).
- Tang, X.; Gao, F.; Xu, G.; Ding, N.; Cai, Y.; Ma, M.; Liu, J. Sensor systems for vehicle environment perception in a highway intelligent space system. Sensors 2014, 14, 8513–8527. [Google Scholar] [CrossRef] [PubMed]
- Abdel-Aty, M.; Oloufa, A.; Peng, Y.; Shen, T.; Yang, X.; Lee, J.; Copley, R.; Ismail, A.; Eady, F.; Lalchan, R.; et al. Real Time Monitoring and Prediction of Reduced Visibility Events on Florida’s Highways; Report, BDV24 962-01; University of Central Florida: Orlando, FL, USA, 2014. [Google Scholar]
- Saponara, S.; Neri, B. Radar Sensor Signal Acquisition and Multidimensional FFT Processing for Surveillance Applications in Transport Systems. IEEE Trans. Instrum. Meas. 2017, 66, 604–615. [Google Scholar] [CrossRef]
- Kschischang, F.R.; Frey, B.J.; Loeliger, H.A. Factor graphs and the sum-product algorithm. IEEE Trans. Inf. Theory 2001, 47, 498–519. [Google Scholar] [CrossRef]
- Loeliger, H.A. An introduction to factor graphs. IEEE Signal Process. Mag. 2004, 21, 28–41. [Google Scholar] [CrossRef]
- Indelman, V.; Williams, S.; Kaess, M.; Dellaert, F. Information fusion in navigation systems via factor graph based incremental smoothing. Robot. Auton. Syst. 2013, 61, 721–738. [Google Scholar] [CrossRef]
- Arras, K.O. An Introduction to Error Propagation: Derivation, Meaning and Examples of Equation CY = FX CXFX T; Technical Report; ETH-Zürich: Zürich, Switzerland, 1998. [Google Scholar]
- Dellaert, F.; Kaess, M. Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing. Int. J. Robot. Res. 2006, 25, 1181–1203. [Google Scholar] [CrossRef]
- Kaess, M.; Johannsson, H.; Roberts, R.; Ila, V.; Leonard, J.; Dellaert, F. iSAM2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, 9–13 May 2011; pp. 3281–3288. [Google Scholar]
- Chiu, H.P.; Zhou, X.S.; Carlone, L.; Dellaert, F.; Samarasekera, S.; Kumar, R. Constrained optimal selection for multi-sensor robot navigation using plug-and-play factor graphs. In Proceedings of the 2014 IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, China, 31 May–7 June 2014; pp. 663–670. [Google Scholar]
- Sünderhauf, N.; Protzel, P. Towards a robust back-end for pose graph slam. In Proceedings of the 2012 IEEE International Conference on Robotics and Automation (ICRA), Saint Paul, MN, USA, 14–18 May 2012; pp. 1254–1261. [Google Scholar]
- Meller, M. Fast clutter cancellation for noise radars via waveform design. IEEE Trans. Aerosp. Electron. Syst. 2014, 50, 2328–2335. [Google Scholar] [CrossRef]
- Akcakaya, M.; Sen, S.; Nehorai, A. A Novel Data-Driven Learning Method for Radar Target Detection in Nonstationary Environments. IEEE Signal Process. Lett. 2016, 23, 762–766. [Google Scholar] [CrossRef]
- Kwon, Y.; Narayanan, R.M.; Rangaswamy, M. Multi-target detection using total correlation for noise radar systems. IEEE Trans. Aerosp. Electron. Syst. 2013, 49, 1251–1262. [Google Scholar] [CrossRef]
- Gulati, D.; Zhang, F.; Malovetz, D.; Clarke, D.; Knoll, A. Robust Cooperative Localization in a dynamic environment Using Factor Graphs and Probability Data Association Filter. In Proceedings of the 20th International Conference on Information Fusion (FUSION), Xi’an, China, 10–13 July 2017. [Google Scholar]
- Sünderhauf, N.; Protzel, P. Switchable constraints for robust pose graph SLAM. In Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vilamoura, Portugal, 7–12 October 2012; pp. 1879–1884. [Google Scholar]
|Number||No Additional Factor||Topology Factor||SME Factor|
|of Vehicles||Without GPS||With GPS||Without GPS||With GPS||Without GPS||With GPS|
© 2017 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/).