Towards adaptive sensor fusion for simultaneous localization and mapping

Autonomous vehicles have been used in a variety of environments which are too hazardous or difficult for a human to operate in safely and reliably over an extended period of time. Among their many applications, exploration missions in dynamic and unstructured environments with noisy measurements are quite commonplace. In the absence of apriori information about the environment, and uncertain localization information, Simultaneous Localization and Mapping (SLAM) algorithms are employed to improve navigation and mapping accuracy. SLAM algorithms accept measurements from proprioceptive sensors, and exteroceptive sensors, independent of each other. These measurements are assumed to be corrupted by noise having Gaussian characteristics with zero bias and a known variance. The measurements obtained are probabilistically fused together using a sensor fusion algorithm. Robotic platforms are ncreasingly being deployed in a wide spectrum of environments with dynamics which are too complicated or impossible to model apriori. Common examples include Unmanned Aerial Vehicles (UAVs) being used for surveillance of windy environments, or Autonomous Underwater Vehicles (AUVs) deployed for mapping the sea bed. The prevalence of unmodelled environmental biases, for instance, sudden gusts of wind for UAVs, or the wake of passing boats for AUVs, complicate the deployment of the robotic system from a laboratory environment to a more complicated real world scenario. Sucessful formulation and implementation of algorithms in laboratory settings may not always translate to success during deployment in real world settings. Furthermore, the increase in prevalence of applications demanding long term robot autonomy ensures that the robotic platform will face degradation of hardware due to wear-andtear over sustained usage. For example, deployment of Autonomous Surface Crafts (ASCs) in marine environments with saline water will result in the motor being encrusted with brine over long periods of time, causing a change in the motion model characteristics, resulting in divergence of pose and map estimates over long intervals of time. Present localization and mapping algorithms are unable to face these challenges in their present form. Correction of state estimates due to extremely accurate exteroceptive sensors may prove sufficient for deployment for short durations of time, but the computational cost incurred due to the feature extraction and data association modules to improve accuracy, makes them a poor choice for most applications demanding long term deployment. Consequently, a more suitable solution for such scenarios is an algorithm incorporating an interoceptive module to increase adaptivity to changing environments by integrating incoming observations from unaffected sensors, in this case, the exteroceptive sensor. Wrongly estimated noise statistics or mismodeled interoceptive models can cause divergence in the state estimate, regardless of the sensor fusion algorithm used, as this dissertation will show. Consequently, the deployment of adaptive sensor fusion based SLAM algorithms, that adapt to changing model noise statistics, is necessitated. This thesis first explores the usage of the Kalman Smoother algorithm to accurately estimate the motion model covariance matrix for the Extended Kalman Filter (EKF). While an initially promising candidate, further analysis reveals its unsuitability to form as the basis of an interoceptive algorithm due to inherent flaws in the formulation. This thesis then reformulates the existing EKF-SLAM and the Factorized Solution to SLAM (FastSLAM) algorithms using the Adaptive Limited Memory Filter (ALMF) into the Adaptive Extended Kalman Filter-SLAM (AEKF-SLAM) and the Adaptive Extended Kalman Filter-FastSLAM (AEKF-FastSLAM) algorithms. The AEKF-SLAM iteratively estimates the process model noise covariance, while the AEKF-FastSLAM iteratively estimates both the process model noise covariance, as well as the bias. Performance of the proposed algorithm is evaluated in a marine environment and further performance gains are obtained by considering unmodeled environmental biases caused by ocean currents or adverse field effects such as those due to the wake of passing ships and boats. The formuations based on the ALMF are both found to have an over-reliance on window size. This drawback is analyzed through extensive simulations which display the variance in performance of the algorithms for different window sizes. An alternative approach using the Gaussian Particle Filter (GPF) is then examined. The prediction density of the Gaussian Particle Filter is favourably compared with the prediction densities of the Extended Kalman Filter (EKF), the particle filter, the Unscented Kalman Filter (UKF) and the Central Difference Kalman Filter (CDKF) using the Kolmogorov Smirnov statistic. The Gaussian Particle Filter is then used to formulate the Gaussian Particle Filter SLAM (GPF-SLAM) algorithm, and the estimation errors are compared with the errors obtained from the EKF-SLAM, UKF-SLAM and FastSLAM algorithms. The comparison is done using a simulated trajectory, as well as data obtained from marine environment. An Adaptive Gaussian Particle Filter based FastSLAM (AGPF-FastSLAM) algorithm is then formulated in which the Gaussian Particle Filter is used to evolve the noise statistics for the FastSLAM prediction density. The bias and covariance of the process model noise is calculated at each step, and fed back into the algorithm during the next step. This approach is found to outperform the EKF-SLAM, FastSLAM and UKF-SLAM algorithm, as well as the AEKF-FastSLAM algorithm discussed previously. This dissertation is then concluded, and avenues for future research in adaptive sensor fusion for SLAM discussed.DOCTOR OF PHILOSOPHY (EEE