Improving the consistency of nonlinear estimators: analysis, algorithms, and applications

Abstract

Autonomous robots are emerging as candidates for performing increasingly complex tasks, such as surveillance and environment monitoring, search and rescue, and planetary exploration. Nonlinear estimation (i.e., estimating the state of a nonlinear system from noisy measurements) arises in all these applications. For instance, robot localization - which is considered as one of the fundamental problems in robotics - seeks to determine the robot's pose (position and orientation) using measurements from onboard sensors (e.g., an odometer and a camera). Another closely-related and important example is target tracking, where the objective is to estimate the target's state using remote sensor observations. Even though many different algorithms, such as the extended Kalman filter (EKF) and the batch maximum a posteriori (MAP) estimator, have been developed for solving these problems, substantial empirical evidence shows that most existing nonlinear estimators tend to become inconsistent (i.e., the state estimates are biased and the error covariance estimates are smaller than the true ones). Moreover, a significant limitation is that the causes of inconsistency have not been sufficiently studied in the literature; if an estimator is inconsistent, the accuracy of its estimates is unknown, which makes the estimator unreliable. The objective of this dissertation is to investigate the main causes of inconsistency of nonlinear estimation and develop new algorithms for improving consistency. As one of the main research thrusts, we study in depth the inconsistency problem in robot localization, including simultaneous localization and mapping (SLAM) and multi-robot cooperative localization (CL). In particular, we show for the first time ever that one fundamental cause of inconsistency is the mismatch between the observability properties of the underlying nonlinear system and the linearized system used by the estimator. By performing observability analysis, we prove that the linearized error-state system used by standard filtering/smoothing algorithms - the EKF, the unscented Kalman filter (UKF), and the sliding-window filter (SWF) - has an observable subspace of higher dimension than that of the underlying nonlinear system. This implies that these estimators gain spurious information (more specifically, about the global orientation) from the measurements, which unjustifiably reduces the uncertainty of the state estimates and causes inconsistency. Based on this key insight, for unobservable nonlinear systems, we propose a novel methodology for designing consistent linearized estimators. Specifically, we develop a family of Observability-Constrained (OC)-estimators - including the OC-EKF, the OC-UKF, and the OC-SWF - whose Jacobians are computed in a way to ensure that the estimator's linearized system model has an observable subspace of the same dimension as that of the underlying nonlinear system. Furthermore, we investigate the inconsistency of estimators for observable nonlinear systems, such as target tracking using distance or bearing measurements, whose cost functions are non-convex and often have multiple local minima. In such cases, we discover that the inconsistency of a standard linearized estimator, such as the EKF, is primarily due to the fact that the estimator is able to find and track only one local minimum. To address this issue, we convert the estimator's nonlinear cost function into polynomial form and employ algebraic geometry techniques to analytically compute all its local minima. These local minima are used as initial estimates by a bank of MAP estimators to efficiently track the most probable hypotheses for the entire state trajectory. Moreover, we adapt this idea to particle filters (PFs) and develop an Analytically-Guided-Sampling (AGS)-PF. Specifically, the AGS-PF employs an analytically-determined Gaussian mixture as proposal distribution which not only takes into account the most recent measurement but also matches all the modes of the posterior (optimal proposal) distribution. As a result, the AGS-PF samples the most probable regions of the state space and hence significantly reduces the number of particles required. As precise long-term localization and tracking are essential for a variety of robotic applications, by introducing a solid theoretical framework for improving the consistency of nonlinear estimators, this work offers significant benefits for robots employed in these tasks. Moreover, the proposed solutions constitute novel paradigms for engineers to follow when designing consistent estimators for other nonlinear systems, and hence have the potential to benefit applications beyond robotics.