An algorithm for estimating the pose, i.e., translation and rotation, of an extended target object is introduced. Compared to conventional methods, where pose estimation is performed on the basis of timeof-flight (TOF) measurements between external sources and sensors attached to the object, the proposed approach directly uses the amplitude values measured at the sensors for estimation purposes without an intermediate TOF estimation step. This is achieved by modeling the wave propagation by a nonlinear dynamic system comprising a system and a measurement equation. The nonlinear system equation includes a model of the time-variant structure of the object rotation based on rotation vectors. As a result, the measured amplitude values at the sensors can be processed instantaneously in a recursive fashion. Uncertainties in the measurement process are systematically considered by employing a stochastic filter for estimating the pose, i.e., the state of the nonlinear dynamic system.
Frederik Beutler, Marco F. Huber, Uwe D. Hanebeck