Rigidity Theory in SE(2) for Unscaled Relative Position Estimation using only Bearing Measurements

This work considers the problem of estimating the unscaled relative positions of a multi-robot team in a common reference frame from bearing-only measurements. Each robot has access to a relative bearing measurement taken from the local body frame of the robot, and the robots have no knowledge of a common or inertial reference frame. A corresponding extension of rigidity theory is made for frameworks embedded in the \emph{special Euclidean group} $SE(2) = \mathbb{R}^2 \times \mathcal{S}^1$. We introduce definitions describing rigidity for $SE(2)$ frameworks and provide necessary and sufficient conditions for when such a framework is \emph{infinitesimally rigid} in $SE(2)$. Analogous to the rigidity matrix for point formations, we introduce the \emph{directed bearing rigidity matrix} and show that an $SE(2)$ framework is infinitesimally rigid if and only if the rank of this matrix is equal to $2|\mathcal{V}|-4$, where $|\mathcal{V}|$ is the number of agents in the ensemble. The directed bearing rigidity matrix and its properties are then used in the implementation and convergence proof of a distributed estimator to determine the {unscaled}{} relative positions in a common frame. Some simulation results are also given to support the analysis.