Cooperative Localization for Multiple Soccer Agents Using Factor Graphs and Sequential Monte Carlo

This paper addresses the cooperative localization problem for a multiagent system in the framework of belief propagation. In particular, we consider the RoboCup 3D Soccer Simulation scenario, in which the networked agents are able to obtain simulated measurements of the distance and bearing to both known landmarks and teammates as well as the direction of arrival (DOA) of messages received from allies around the field. There are, however, severe communication restrictions between the agents, which limit the size and periodicity of the information that can be exchanged between them. We factorize the joint probability density function of the state of the robots conditioned on all measurements in the network in order to derive the corresponding factor graph representation of the cooperative localization problem. Then we apply the sum-product-algorithm (SPA) and introduce suitable implementations thereof using hybrid Gaussian-Mixture Model (GMM) / Sequential Monte Carlo (SMC) representations of the individual messages that are passed at each network location. Simulated results show that the cooperative estimates for position and orientation converge faster and present smaller errors when compared to the non-cooperative estimates in situations where agents do not observe landmarks for a long period.