Towards Robust and Accurate Cooperative State Estimation for Multiple Rigid Bodies

Estimating the state (i.e., determining the position and orientation) of a rigid body (RB) is a key enabler technology for many applications (e.g., vehicle motion control and autonomous driving). Thus, rigid body localization (RBL), which can obtain both positions and poses of RBs, has drawn great attention recently. Most existing RBL methods mainly focus on estimating a single RB's state. However, the multi-agent scenario, which can be found in many applications like vehicle-to-vehicle communication, has not been explicitly accounted for. Moreover, these methods undergo significant performance degradation when the communication conditions between sensors and anchors are poor. In this work, we propose a cooperative rigid body localization (CRBL) framework, which additionally leverages the interactions among RBs to achieve accurate and robust state estimation of multiple RBs. To this end, we need to figure out two key questions: 1) how to assess the attainable performance of CRBL; 2) how to use the newly introduced measurements among RBs to enhance the estimation performance. To answer the two questions, we first perform the Cramer-Rao lower bound (CRLB) analysis for the CRBL framework to characterize the performance improvement. Theoretical analysis shows that CRBL has a lower error bound than the conventional RBL framework. Furthermore, we propose two efficient CRBL algorithms. The first one is a lightweight method based on the weighted least squares, named WLS-CRBL, which is an optimal algorithm over the small noise region and is computationally efficient. The second one is implemented by the semi-definite relaxation (SDR) technique named SDR-CRBL, aiming to achieve robust estimation when the noise is high. Numerical experiments show that the proposed methods can achieve their theoretical performance and outperform the conventional RBL algorithms, especially in the case of poor communications between sensors and anchors.