Unscented iSAM: A consistent incremental solution to cooperative localization and target tracking

In this paper, we study the problem of cooperative localization and target tracking (CLATT), i.e., a team of mobile robots use their onboard sensors' measurements to cooperatively track multiple moving targets, and propose a novel unscented incremental smoothing and mapping (U-iSAM) approach. The proposed method attains reduced linearization errors by using the unscented transformation and correct observability properties by imposing observability constraints on the unscented transformation when computing measurement Jacobians. In particular, we, for the first time ever, analyze the observability properties of the batch maximum a posteriori (MAP)-based CLATT system, and show that in the case of no prior, the Hessian (information) matrix has a nullspace of dimension three. However, this may not be the case when the Jacobians (and thus the Hessian) are computed numerically through the unscented transformation. To ensure that the U-iSAM possesses correct observability (i.e., the nullspace of its Hessian is of dimension three), we project the measurement Jacobians computed by the standard unscented transformation onto the observable subspace. The proposed algorithm is validated through extensive Monte-Carlo simulations.