Consensus-based Distributed 3D Pose Estimation with Noisy Relative Measurements

In this paper we study consensus-based tributed estimation algorithms for estimating the global translation and rotation of each agent in a multi-agent system. We consider the case in which agents measure the noisy relative pose of their neighbors and communicate their estimates to agree upon the global poses in an arbitrary reference frame. The main contribution of this paper is a formal analysis that provides necessary and sufficient conditions to guarantee stability (in a Lyapunov sense) of the estimation system's equilibria. We prove that consensus-based algorithms will diverge, even with arbitrarily small inconsistencies on the relative pose, unless the measurements satisfy minimum consistency conditions. We determine these consistency conditions for translation-only, rotation-only, and combined 3D pose estimation using the axis-angle rotation representation over undirected graphs. We then propose an initialization method based on these conditions that guarantees consistency and stability of the estimator's equilibria. Additionally, we show that existing distributed estimation methods in literature exploit these conditions to guarantee convergence of their algorithms. Lastly, we perform simulations that show convergence when consistency conditions hold and divergence when they do not.