Multi-vehicle localisation with additive compressed factor graphs

This paper introduces a distributed and decentralised method to solve the cooperative localisation problem utilising the factor graph framework. This method enables vehicles to create compact packets of their own sensor information between their involvement in intervehicle measurements. The packets are limited in size, with this size dependent on the size of the state space alone. The number of packets generated is also limited at two packets (one per vehicle) created per intervehicle measurement. The packets and measurements can be shared and propagated to all vehicles not just direct neighbours or to vehicles involved in the measurements. Vehicles maintain a local solver that incorporates all local sensor information and motion updates in full nonlinear form and includes the fixed linearised packets from other vehicles. This local solver is able to update local state variables and relinearise local and intervehicle factors but has to hold remote state variables and packet data at a fixed linearisation point. We show the estimated solution is of a similar quality to a state of the art centralised relinearising estimator iSAM2 and superior to a fixed linearisation filtering solution via comparing RMS error in position estimation in simulation. Consistency of the method is also shown via the NEES metric. Communication requirements for each of the competing methods are shown, with compaction of packets being more useful the larger the ratio between intervehicle and local measurement intervals. The technique is validated using multi-vehicle simulation and real datasets.