Collaborative localization with heterogeneous inter-robot measurements by Riemannian optimization

We propose a distributed algorithm for collaborative localization of multiple autonomous robots that fuses inter-robot relative measurements with odometry measurements to improve upon dead reckoning estimates. It is an extension of our previous work [6], in which a method for fusing inter-robot pose measurements was presented. In this paper we extend the method to fuse any type of inter-robot measurements (distance, bearing, relative position, relative orientation, and any combination thereof), thus increasing the applicability of the method. The proposed method is posed as an optimization problem in a product Riemannian manifold; and is solved by gradient descent without performing a parameterization of the orientations. The proposed distributed algorithm allows each robot to compute its own pose estimate based on local measurements and communication with its neighbors. Simulations show that the proposed distributed algorithm significantly improves localization accuracy over the case of no-collaboration. Simulations show that, in some situations, the proposed distributed algorithm outperforms two competing methods - an Extended Kalman Filter-based algorithm as well as a distributed pose graph optimization method that relies on an Euclidean parameterization of orientations.