Joint tracking-registration with linear complexity: An application to range sensors

Sensor fusion of multiple sources plays an important role in robotic systems to achieve refined target position and velocity estimates. In this paper, we address the general registration problem, which is a key module for a fusion system to accurately correct systematic errors of sensors. A fast maximum a posteriori (FMAP) algorithm for joint registration and tracking is presented. The algorithm uses a recursive two-step optimization that involves orthogonal factorization to ensure numerically stability. Statistical efficiency analysis based on Cramer-Rao lower bound theory is presented to show asymptotical optimality of FMAP. Also, Givens rotation is used to derive a fast implementation with complexity O(n) (n denoting number of targets). Experiments are presented to demonstrate the promise and effectiveness of FMAP.