The Estimation Theoretic Sensor Bias Correction Problem in Map Aided Localization

Abstract

Simultaneous Localization and Map Building (SLAM) and Map Aided Localization (MAL) are very effective techniques employed extensively in robot navigation tasks. However, biases and drifts in both exteroceptive and proprioceptive sensors adversely impair correct localization (in MAL) and also impair map building (in SLAM). More specifically, accumulated errors as a result of biases in the sensors cause the algorithms to diverge and produce inconsistent and inaccurate results. Although offline calibration of these sensors can reduce the effects to some extent, the process results in longer setup and processing times. Moreover, during operation, the sensors’ calibration may often be subject to changes or drifts requiring regular resetting and initialization. A convenient, appropriate and effective approach to overcome problems associated with biases in sensors has been to explicitly model and estimate the bias parameters concurrently with the vehicle state online using an augmented state space approach. This paper investigates the properties of the concurrent bias estimation in MAL using an augmented, estimation theoretic state space approach for the localization of a large class of mobile robots, consisting of autonomous ground vehicles. This involves a rigorous theoretical study of the issues of observability and convergence, their interrelations and effects on the algorithm’s performance. This paper shows analytically that if sensor biases are estimated jointly with the vehicle pose in a MAL framework: 1) The uncertainties of the estimated errors in the bias parameters of both proprioceptive and exteroceptive sensors diminish in each update. 2) A derived lower bound is reached in each of these estimates. 3) The rate of convergence to this lower bound is also derived. 4) Although often neglected in the literature, observability is a major issue. From the analysis it is derived that in order to guarantee observability in MAL with bias estimation, it is necessary to observe simultaneously at least two distinct landmarks, which are not on a straight line with the vehicle position. Extensive simulations are provided to illustrate the theoretical results established for the general case of nonlinear dynamics and slowly varying sensor biases. The results are further exemplified and verified experimentally using a sophisticated MAL algorithm, utilizing a low cost inertial navigation sensor suite.