Collaborative Pose Filtering Using Relative Measurements and Communications

This paper presents a collaborative pose filtering algorithm for distributed localization of a team of heterogeneous robots moving in 3D space. Each robot estimates its own pose (attitude and translation and associated covariance matrices) with respect to a global reference frame using velocity measurements, absolute measurements, relative inter-vehicle measurements and communicated pose estimates of nearby vehicles. Our distributed filtering mechanism is derived based on distributed nonlinear least squares optimization, and transforms relative measurements and communicated pose estimates obtained from nearby vehicles into the form of landmark measurements. The resulting measurements can readily be utilized with any pose filtering algorithm to achieve distributed localization of individual vehicles. The proposed approach is compatible with a range of covariance tuning approaches. In this paper the Geometric Approximate Minimum Energy (GAME) filter on the special Euclidean group SE(3) is utilized as the pose filtering algorithm. A continuous-discrete version of the GAME filter is provided that is able to update the continuously predicted pose estimate with the absolute and relative measurements obtained in discrete time. We validate the proposed algorithms in simulation, showing platforms can achieve localization with minimal landmark measurements.