Fast Optimal Joint Tracking–Registration for Multisensor Systems

Sensor fusion of multiple sources plays an important role in vehicular 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-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. In addition, Givens rotation is used to derive a fast implementation with complexity O(n), with n being the number of tracked targets. Simulations and experiments are presented to demonstrate the promise and effectiveness of FMAP.