Position and orientation estimation of a rigid body: Rigid body localization

Rigid body localization refers to a problem of estimating the position of a rigid body along with its orientation using anchors. We consider a setup in which a few sensors are mounted on a rigid body. The absolute position of the rigid body is not known, but, the relative position of the sensors or the topology of the sensors on the rigid body is known. We express the absolute position of the sensors as an affine function of the Stiefel manifold and propose a simple least-squares (LS) estimator as well as a constrained total least-squares (CTLS) estimator to jointly estimate the orientation and the position of the rigid body. To account for the perturbations of the sensors, we also propose a constrained total least-squares (CTLS) estimator. Analytical closed-form solutions for the proposed estimators are provided. Simulations are used to corroborate and analyze the performance of the proposed estimators.