Graph based cooperative localization using symmetric measurement equations and dedicated short range communication

Safety is one of the critical challenges for a semi or fully automated driving assistance systems. One of the key parameters for a safe automated driving assistance system is precise localization of the self and surrounding vehicles. Our previous work demonstrated the use of Symmetric Measurement Equations (SME) in a Factor Graph framework and exploited the use of sensor located outside the vehicle. In this paper we present a new approach which not only performs above mentioned cooperative vehicle infrastructure localization but also uses Dedicated Short Range Communication (DSRC). This work goes further in the direction of a complete V2X solution not only involving the infrastructure sensor but also the neighbouring vehicles. DSRC has been increasingly incorporated in all the the modern vehicles. Better state estimation is achieved by formulating the range information from DSRC as a new DSRC Range Factor in the Factor Graph. Simulations indicate better performance over the previously proposed approach of only using SME in the Factor Graph, thereby progressing a step further towards safe automated driving assistance systems.