A fundamental problem of non-linear state estimation in robotics is the violation of assumptions about the sensors' error distribution. State of the art approaches reduce the impact of these ...violations with robust cost functions or predefined non-Gaussian error models. Both require extensive parameter tuning and fail if the sensors' error characteristic changes over time, due to environmental changes, ageing or sensor malfunctions. We demonstrate how the error distribution itself can be part of the state estimation process. Based on an efficient approximation of a Gaussian mixture, we optimize the sensor model simultaneously during the standard state estimation. Due to an implicit expectation-maximization approach, we achieve a fast convergence without prior knowledge of the true distribution parameters. We implement this self-tuning algorithm in a least-squares optimization framework and demonstrate its real time capability on a real world dataset for satellite localization of a driving vehicle. The resulting estimation quality is superior to previous robust algorithms.
Factor graph optimization (FGO) recently has attracted attention as an alternative to the extended Kalman filter (EKF) for GNSS‐INS integration. This study evaluates both loosely and tightly coupled ...integrations of GNSS code pseudorange and INS measurements for real‐time positioning, using both conventional EKF and FGO with a dataset collected in an urban canyon in Hong Kong. The FGO strength is analyzed by degenerating the FGO‐based estimator into an “EKF‐like estimator.” In addition, the effects of window size on FGO performance are evaluated by considering both the GNSS pseudorange error models and environmental conditions. We conclude that the conventional FGO outperforms the EKF because of the following two factors: (1) FGO uses multiple iterations during the estimation to achieve a robust estimation; and (2) FGO better explores the time correlation between the measurements and states, based on a batch of historical data, when the measurements do not follow the Gaussian noise assumption.
GNSS localization is an important part of today's autonomous systems, although it suffers from non-Gaussian errors caused by non-line-of-sight effects. Recent methods are able to mitigate these ...effects by including the corresponding distributions in the sensor fusion algorithm. However, these approaches require prior knowledge about the sensor's distribution, which is often not available. We introduce a novel sensor fusion algorithm based on variational Bayesian inference, that is able to approximate the true distribution with a Gaussian mixture model and to learn its parametrization online. The proposed Incremental Variational Mixture algorithm automatically adapts the number of mixture components to the complexity of the measurement's error distribution. We compare the proposed algorithm against current state-of-the-art approaches using a collection of open access real world datasets and demonstrate its superior localization accuracy.
Gaussian mixtures are a powerful and widely used tool to model non-Gaussian estimation problems. They are able to describe measurement errors that follow arbitrary distributions and can represent ...ambiguity in assignment tasks like point set registration or tracking. However, using them with common least squares solvers is still difficult. Existing approaches are either approximations of the true mixture or prone to convergence issues due to their strong nonlinearity. We propose a novel least squares representation of a Gaussian mixture, which is an exact and almost linear model of the corresponding log-likelihood. Our approach provides an efficient, accurate and flexible model for many probabilistic estimation problems and can be used as cost function for least squares solvers. We demonstrate its superior performance in various Monte Carlo experiments, including different kinds of point set registration. Our implementation is available as open source code for the state-of-the-art solvers Ceres and GTSAM.
Accurate and reliable tracking of multiple moving objects in 3D space is an essential component of urban scene understanding. This is a challenging task because it requires the assignment of ...detections in the current frame to the predicted objects from the previous one. Existing filter-based approaches tend to struggle if this initial assignment is not correct, which can happen easily.We propose a novel optimization-based approach that does not rely on explicit and fixed assignments. Instead, we represent the result of an off-the-shelf 3D object detector as Gaussian mixture model, which is incorporated in a factor graph framework. This gives us the flexibility to assign all detections to all objects simultaneously. As a result, the assignment problem is solved implicitly and jointly with the 3D spatial multi-object state estimation using non-linear least squares optimization.Despite its simplicity, the proposed algorithm achieves robust and reliable tracking results and can be applied for offline as well as online tracking. We demonstrate its performance on the real world KITTI tracking dataset and achieve better results than many state-of-the-art algorithms. Especially the consistency of the estimated tracks is superior offline as well as online.
Consistent motion estimation is fundamental for all mobile autonomous systems. While this sounds like an easy task, often, it is not the case because of changing environmental conditions affecting ...odometry obtained from vision, Lidar, or the wheels themselves. Unsusceptible to challenging lighting and weather conditions, radar sensors are an obvious alternative. Usually, automotive radars return a sparse point cloud, representing the surroundings. Utilizing this information to motion estimation is challenging due to unstable and phantom measurements, which result in a high rate of outliers. We introduce a credible and robust probabilistic approach to estimate the ego-motion based on these challenging radar measurements; intended to be used within a loosely-coupled sensor fusion framework. Compared to existing solutions, evaluated on the popular nuScenes dataset and others, we show that our proposed algorithm is more credible while not depending on explicit correspondence calculation.
Robust and reliable online 3D multi-object tracking is an essential component of autonomous driving. Recent research follows the tracking-by-detection paradigm and focuses mainly on lidar sensors, ...due to their superior range, resolution and depth accuracy compared to other automotive sensors. This simplifies the challenging data association in crowded urban road scenes, resulting in a predominant status of laser based methods. In contrast, we propose an online 3D multi-object tracker based solely on mono camera images and radar data to promote non-lidar based tracking research. By representing all detections of one frame as a Gaussian mixture model (GMM), we are able to avoid a fixed data association, which may include wrong assumptions. Instead, we assign the GMM to each tracked object and solve the data association implicitly and jointly by estimating the full 3D object tracks in our factor graph based optimization back end. By including all available information from the object detector, our algorithm achieves accurate, robust and reliable tracking results. We conduct real world experiments on the nuScenes tracking data set improving the state-of-the-art for non-lidar based methods from 17.7% to 34.1 % AMOTA.
Accurate and globally referenced positioning is crucial to autonomous systems with navigation requirements, such as unmanned aerial vehicles (UAV) and autonomous driving vehicles (ADV). GNSS/LiDAR ...integration is a popular sensor pair that can provide outstanding positioning performance in open areas. However, the accuracy is significantly degraded in urban canyons, due to the excessive unmodeled non-Gaussian GNSS outliers caused by multipath effects and none-line-of-sight (NLOS) receptions. As a result, the violation of the Gaussian assumption can severely distort the sensor fusion process, such as the extended Kalman filter (EKF). To mitigate the effects of these non-Gaussian GNSS outliers, this paper proposes to leverage the Gaussian mixture model (GMM) to describe the potential noise of GNSS positioning and apply it to further sensor fusion. Instead of relying on excessive offline parameterization and tuning, the parameters of the GMM are estimated simultaneously based on the residuals of the GNSS measurements using an expectation-maximization (EM) algorithm. Then the state-of-the-art factor graph optimization (FGO) is applied to integrate the GNSS positioning and LiDAR odometry based on the estimated GMM. The experiment in a typical urban canyon is conducted to validate the performance of the proposed method. The result shows that the GMM can effectively mitigate the effects of GNSS outliers and improves positioning performance.
Non-Gaussian and multimodal distributions are an important part of many recent robust sensor fusion algorithms. In difference to robust cost functions, they are probabilistically founded and have ...good convergence properties. Since their robustness depends on a close approximation of the real error distribution, their parametrization is crucial.We propose a novel approach that allows to adapt a multi-modal Gaussian mixture model to the error distribution of a sensor fusion problem. By combining expectation-maximization and non-linear least squares optimization, we are able to provide a computationally efficient solution with well-behaved convergence properties.We demonstrate the performance of these algorithms on several real-world GNSS and indoor localization datasets. The proposed adaptive mixture algorithm outperforms state-of-the-art approaches with static parametrization. Source code and datasets are available under https://mytuc.org/libRSF.
While many applications of sensor fusion suffer from the occurrence of outliers, a broad range of outlier robust graph optimization techniques has been developed for simultaneous localization and ...mapping. In this paper we investigate the performance of some of the most advanced algorithms for a simulated wireless localization setting affected by non-Gaussian errors. With this first analysis we can show some of the advantages and disadvantages that are connected with the different concepts behind Max-Mixture, Generalized iSAM, Switchable Constraints and Dynamic Covariance Scaling.
PDF
