-
Invariant filtering for wheeled vehicle localization with unknown wheel radius and unknown GNSS lever arm
Authors:
Paul Chauchat,
Silvère Bonnabel,
Axel Barrau
Abstract:
We consider the problem of observer design for a nonholonomic car (more generally a wheeled robot) equipped with wheel speeds with unknown wheel radius, and whose position is measured via a GNSS antenna placed at an unknown position in the car. In a tutorial and unified exposition, we recall the recent theory of two-frame systems within the field of invariant Kalman filtering. We then show how to…
▽ More
We consider the problem of observer design for a nonholonomic car (more generally a wheeled robot) equipped with wheel speeds with unknown wheel radius, and whose position is measured via a GNSS antenna placed at an unknown position in the car. In a tutorial and unified exposition, we recall the recent theory of two-frame systems within the field of invariant Kalman filtering. We then show how to adapt it geometrically to address the considered problem, although it seems at first sight out of its scope. This yields an invariant extended Kalman filter having autonomous error equations, and state-independent Jacobians, which is shown to work remarkably well in simulations. The proposed novel construction thus extends the application scope of invariant filtering.
△ Less
Submitted 11 September, 2024;
originally announced September 2024.
-
Backpropagation-Based Analytical Derivatives of EKF Covariance for Active Sensing
Authors:
Jonas Benhamou,
Silvère Bonnabel,
Camille Chapdelaine
Abstract:
To enhance accuracy of robot state estimation, active sensing (or perception-aware) methods seek trajectories that maximize the information gathered by the sensors. To this aim, one possibility is to seek trajectories that minimize the (estimation error) covariance matrix output by an extended Kalman filter (EKF), w.r.t. its control inputs over a given horizon. However, this is computationally dem…
▽ More
To enhance accuracy of robot state estimation, active sensing (or perception-aware) methods seek trajectories that maximize the information gathered by the sensors. To this aim, one possibility is to seek trajectories that minimize the (estimation error) covariance matrix output by an extended Kalman filter (EKF), w.r.t. its control inputs over a given horizon. However, this is computationally demanding. In this article, we derive novel backpropagation analytical formulas for the derivatives of the covariance matrices of an EKF w.r.t. all its inputs. We then leverage the obtained analytical gradients as an enabling technology to derive perception-aware optimal motion plans. Simulations validate the approach, showcasing improvements in execution time, notably over PyTorch's automatic differentiation. Experimental results on a real vehicle also support the method.
△ Less
Submitted 8 March, 2024; v1 submitted 27 February, 2024;
originally announced February 2024.
-
Variational Gaussian approximation of the Kushner optimal filter
Authors:
Marc Lambert,
Silvère Bonnabel,
Francis Bach
Abstract:
In estimation theory, the Kushner equation provides the evolution of the probability density of the state of a dynamical system given continuous-time observations. Building upon our recent work, we propose a new way to approximate the solution of the Kushner equation through tractable variational Gaussian approximations of two proximal losses associated with the propagation and Bayesian update of…
▽ More
In estimation theory, the Kushner equation provides the evolution of the probability density of the state of a dynamical system given continuous-time observations. Building upon our recent work, we propose a new way to approximate the solution of the Kushner equation through tractable variational Gaussian approximations of two proximal losses associated with the propagation and Bayesian update of the probability density. The first is a proximal loss based on the Wasserstein metric and the second is a proximal loss based on the Fisher metric. The solution to this last proximal loss is given by implicit updates on the mean and covariance that we proposed earlier. These two variational updates can be fused and shown to satisfy a set of stochastic differential equations on the Gaussian's mean and covariance matrix. This Gaussian flow is consistent with the Kalman-Bucy and Riccati flows in the linear case and generalize them in the nonlinear one.
△ Less
Submitted 3 October, 2023;
originally announced October 2023.
-
Invariant Smoothing for Localization: Including the IMU Biases
Authors:
Paul Chauchat,
Silvère Bonnabel,
Axel Barrau
Abstract:
In this article we investigate smoothing (i.e., optimisation-based) estimation techniques for robot localization using an IMU aided by other localization sensors. We more particularly focus on Invariant Smoothing (IS), a variant based on the use of nontrivial Lie groups from robotics. We study the recently introduced Two Frames Group (TFG), and prove it can fit into the framework of Invariant Smoo…
▽ More
In this article we investigate smoothing (i.e., optimisation-based) estimation techniques for robot localization using an IMU aided by other localization sensors. We more particularly focus on Invariant Smoothing (IS), a variant based on the use of nontrivial Lie groups from robotics. We study the recently introduced Two Frames Group (TFG), and prove it can fit into the framework of Invariant Smoothing in order to better take into account the IMU biases, as compared to the state-of-the-art in robotics. Experiments based on the KITTI dataset show the proposed framework compares favorably to the state-of-the-art smoothing methods in terms of robustness in some challenging situations.
△ Less
Submitted 4 September, 2024; v1 submitted 25 September, 2023;
originally announced September 2023.
-
The limited-memory recursive variational Gaussian approximation (L-RVGA)
Authors:
Marc Lambert,
Silvère Bonnabel,
Francis Bach
Abstract:
We consider the problem of computing a Gaussian approximation to the posterior distribution of a parameter given a large number N of observations and a Gaussian prior, when the dimension of the parameter d is also large. To address this problem we build on a recently introduced recursive algorithm for variational Gaussian approximation of the posterior, called recursive variational Gaussian approx…
▽ More
We consider the problem of computing a Gaussian approximation to the posterior distribution of a parameter given a large number N of observations and a Gaussian prior, when the dimension of the parameter d is also large. To address this problem we build on a recently introduced recursive algorithm for variational Gaussian approximation of the posterior, called recursive variational Gaussian approximation (RVGA), which is a single pass algorithm, free of parameter tuning. In this paper, we consider the case where the parameter dimension d is high, and we propose a novel version of RVGA that scales linearly in the dimension d (as well as in the number of observations N), and which only requires linear storage capacity in d. This is afforded by the use of a novel recursive expectation maximization (EM) algorithm applied for factor analysis introduced herein, to approximate at each step the covariance matrix of the Gaussian distribution conveying the uncertainty in the parameter. The approach is successfully illustrated on the problems of high dimensional least-squares and logistic regression, and generalized to a large class of nonlinear models.
△ Less
Submitted 24 March, 2023;
originally announced March 2023.
-
Variational inference via Wasserstein gradient flows
Authors:
Marc Lambert,
Sinho Chewi,
Francis Bach,
Silvère Bonnabel,
Philippe Rigollet
Abstract:
Along with Markov chain Monte Carlo (MCMC) methods, variational inference (VI) has emerged as a central computational approach to large-scale Bayesian inference. Rather than sampling from the true posterior $π$, VI aims at producing a simple but effective approximation $\hat π$ to $π$ for which summary statistics are easy to compute. However, unlike the well-studied MCMC methodology, algorithmic g…
▽ More
Along with Markov chain Monte Carlo (MCMC) methods, variational inference (VI) has emerged as a central computational approach to large-scale Bayesian inference. Rather than sampling from the true posterior $π$, VI aims at producing a simple but effective approximation $\hat π$ to $π$ for which summary statistics are easy to compute. However, unlike the well-studied MCMC methodology, algorithmic guarantees for VI are still relatively less well-understood. In this work, we propose principled methods for VI, in which $\hat π$ is taken to be a Gaussian or a mixture of Gaussians, which rest upon the theory of gradient flows on the Bures--Wasserstein space of Gaussian measures. Akin to MCMC, it comes with strong theoretical guarantees when $π$ is log-concave.
△ Less
Submitted 21 April, 2023; v1 submitted 31 May, 2022;
originally announced May 2022.
-
Invariant Smoothing with low process noise
Authors:
Paul Chauchat,
Silvere Bonnabel,
Axel Barrau
Abstract:
In this paper we address smoothing-that is, optimisation-based-estimation techniques for localisation problems in the case where motion sensors are very accurate. Our mathematical analysis focuses on the difficult limit case where motion sensors are infinitely precise, resulting in the absence of process noise. Then the formulation degenerates, as the dynamical model that serves as a soft constrai…
▽ More
In this paper we address smoothing-that is, optimisation-based-estimation techniques for localisation problems in the case where motion sensors are very accurate. Our mathematical analysis focuses on the difficult limit case where motion sensors are infinitely precise, resulting in the absence of process noise. Then the formulation degenerates, as the dynamical model that serves as a soft constraint becomes an equality constraint, and conventional smoothing methods are not able to fully respect it. By contrast, once an appropriate Lie group embedding has been found, we prove theoretically that invariant smoothing gracefully accommodates this limit case in that the estimates tend to be consistent with the induced constraints when the noise tends to zero. Simulations on the important problem of initial alignement in inertial navigation show that, in a low noise setting, invariant smoothing may favorably compare to state-of-the-art smoothers when using precise inertial measurements units (IMU).
△ Less
Submitted 5 April, 2022;
originally announced April 2022.
-
The Geometry of Navigation Problems
Authors:
Axel Barrau,
Silvere Bonnabel
Abstract:
While many works exploiting an existing Lie group structure have been proposed for state estimation, in particular the Invariant Extended Kalman Filter (IEKF), few papers address the construction of a group structure that allows casting a given system into the framework of invariant filtering. In this paper we introduce a large class of systems encompassing most problems involving a navigating veh…
▽ More
While many works exploiting an existing Lie group structure have been proposed for state estimation, in particular the Invariant Extended Kalman Filter (IEKF), few papers address the construction of a group structure that allows casting a given system into the framework of invariant filtering. In this paper we introduce a large class of systems encompassing most problems involving a navigating vehicle encountered in practice. For those systems we introduce a novel methodology that systematically provides a group structure for the state space, including vectors of the body frame such as biases. We use it to derive observers having properties akin to those of linear observers or filters. The proposed unifying and versatile framework encompasses all systems where IEKF has proved successful, improves state-of-the art "imperfect" IEKF for inertial navigation with sensor biases, and allows addressing novel examples, like GNSS antenna lever arm estimation.
△ Less
Submitted 6 February, 2022; v1 submitted 12 January, 2022;
originally announced January 2022.
-
Factor Graph-Based Smoothing Without Matrix Inversion for Highly Precise Localization
Authors:
Paul Chauchat,
Axel Barrau,
Silvère Bonnabel
Abstract:
We consider the problem of localizing a manned, semi-autonomous, or autonomous vehicle in the environment using information coming from the vehicle's sensors, a problem known as navigation or simultaneous localization and mapping (SLAM) depending on the context. To infer knowledge from sensors' measurements, while drawing on a priori knowledge about the vehicle's dynamics, modern approaches solve…
▽ More
We consider the problem of localizing a manned, semi-autonomous, or autonomous vehicle in the environment using information coming from the vehicle's sensors, a problem known as navigation or simultaneous localization and mapping (SLAM) depending on the context. To infer knowledge from sensors' measurements, while drawing on a priori knowledge about the vehicle's dynamics, modern approaches solve an optimization problem to compute the most likely trajectory given all past observations, an approach known as smoothing. Improving smoothing solvers is an active field of research in the SLAM community. Most work is focused on reducing computation load by inverting the involved linear system while preserving its sparsity. The present paper raises an issue which, to the knowledge of the authors, has not been addressed yet: standard smoothing solvers require explicitly using the inverse of sensor noise covariance matrices. This means the parameters that reflect the noise magnitude must be sufficiently large for the smoother to properly function. When matrices are close to singular, which is the case when using high precision modern inertial measurement units (IMU), numerical issues necessarily arise, especially with 32-bits implementation demanded by most industrial aerospace applications. We discuss these issues and propose a solution that builds upon the Kalman filter to improve smoothing algorithms. We then leverage the results to devise a localization algorithm based on fusion of IMU and vision sensors. Successful real experiments using an actual car equipped with a tactical grade high performance IMU and a LiDAR illustrate the relevance of the approach to the field of autonomous vehicles.
△ Less
Submitted 22 September, 2020;
originally announced September 2020.
-
Associating Uncertainty to Extended Poses for on Lie Group IMU Preintegration with Rotating Earth
Authors:
Martin Brossard,
Axel Barrau,
Paul Chauchat,
Silvère Bonnabel
Abstract:
The recently introduced matrix group SE2(3) provides a 5x5 matrix representation for the orientation, velocity and position of an object in the 3-D space, a triplet we call "extended pose". In this paper we build on this group to develop a theory to associate uncertainty with extended poses represented by 5x5 matrices. Our approach is particularly suited to describe how uncertainty propagates when…
▽ More
The recently introduced matrix group SE2(3) provides a 5x5 matrix representation for the orientation, velocity and position of an object in the 3-D space, a triplet we call "extended pose". In this paper we build on this group to develop a theory to associate uncertainty with extended poses represented by 5x5 matrices. Our approach is particularly suited to describe how uncertainty propagates when the extended pose represents the state of an Inertial Measurement Unit (IMU). In particular it allows revisiting the theory of IMU preintegration on manifold and reaching a further theoretic level in this field. Exact preintegration formulas that account for rotating Earth, that is, centrifugal force and Coriolis force, are derived as a byproduct, and the factors are shown to be more accurate. The approach is validated through extensive simulations and applied to sensor-fusion where a loosely-coupled fixed-lag smoother fuses IMU and LiDAR on one hour long experiments using our experimental car. It shows how handling rotating Earth may be beneficial for long-term navigation within incremental smoothing algorithms.
△ Less
Submitted 8 January, 2021; v1 submitted 28 July, 2020;
originally announced July 2020.
-
A Mathematical Framework for IMU Error Propagation with Applications to Preintegration
Authors:
Axel Barrau,
Silvere Bonnabel
Abstract:
To fuse information from inertial measurement units (IMU) with other sensors one needs an accurate model for IMU error propagation in terms of position, velocity and orientation, a triplet we call extended pose. In this paper we leverage a nontrivial result, namely log-linearity of inertial navigation equations based on the recently introduced Lie group $SE_2(3)$, to transpose the recent methodolo…
▽ More
To fuse information from inertial measurement units (IMU) with other sensors one needs an accurate model for IMU error propagation in terms of position, velocity and orientation, a triplet we call extended pose. In this paper we leverage a nontrivial result, namely log-linearity of inertial navigation equations based on the recently introduced Lie group $SE_2(3)$, to transpose the recent methodology of Barfoot and Furgale for associating uncertainty with poses (position, orientation) of $SE(3)$ when using noisy wheel speeds, to the case of extended poses (position, velocity, orientation) of $SE_2(3)$ when using noisy IMUs. Besides, our approach to extended poses combined with log-linearity property allows revisiting the theory of preintegration on manifolds and reaching a further theoretic level in this field. We show exact preintegration formulas that account for rotating Earth, that is, centrifugal force and Coriolis effect, may be derived as a byproduct.
△ Less
Submitted 25 May, 2020; v1 submitted 8 March, 2020;
originally announced March 2020.
-
Denoising IMU Gyroscopes with Deep Learning for Open-Loop Attitude Estimation
Authors:
Martin Brossard,
Silvere Bonnabel,
Axel Barrau
Abstract:
This paper proposes a learning method for denoising gyroscopes of Inertial Measurement Units (IMUs) using ground truth data, and estimating in real time the orientation (attitude) of a robot in dead reckoning. The obtained algorithm outperforms the state-of-the-art on the (unseen) test sequences. The obtained performances are achieved thanks to a well-chosen model, a proper loss function for orien…
▽ More
This paper proposes a learning method for denoising gyroscopes of Inertial Measurement Units (IMUs) using ground truth data, and estimating in real time the orientation (attitude) of a robot in dead reckoning. The obtained algorithm outperforms the state-of-the-art on the (unseen) test sequences. The obtained performances are achieved thanks to a well-chosen model, a proper loss function for orientation increments, and through the identification of key points when training with high-frequency inertial data. Our approach builds upon a neural network based on dilated convolutions, without requiring any recurrent neural network. We demonstrate how efficient our strategy is for 3D attitude estimation on the EuRoC and TUM-VI datasets. Interestingly, we observe our dead reckoning algorithm manages to beat top-ranked visual-inertial odometry systems in terms of attitude estimation although it does not use vision sensors. We believe this paper offers new perspectives for visual-inertial localization and constitutes a step toward more efficient learning methods involving IMUs. Our open-source implementation is available at https://github.com/mbrossar/denoise-imu-gyro.
△ Less
Submitted 26 June, 2020; v1 submitted 25 February, 2020;
originally announced February 2020.
-
A Code for Unscented Kalman Filtering on Manifolds (UKF-M)
Authors:
Martin Brossard,
Axel Barrau,
Silvere Bonnabel
Abstract:
The present paper introduces a novel methodology for Unscented Kalman Filtering (UKF) on manifolds that extends previous work by the authors on UKF on Lie groups. Beyond filtering performance, the main interests of the approach are its versatility, as the method applies to numerous state estimation problems, and its simplicity of implementation for practitioners not being necessarily familiar with…
▽ More
The present paper introduces a novel methodology for Unscented Kalman Filtering (UKF) on manifolds that extends previous work by the authors on UKF on Lie groups. Beyond filtering performance, the main interests of the approach are its versatility, as the method applies to numerous state estimation problems, and its simplicity of implementation for practitioners not being necessarily familiar with manifolds and Lie groups. We have developed the method on two independent open-source Python and Matlab frameworks we call UKF-M, for quickly implementing and testing the approach. The online repositories contain tutorials, documentation, and various relevant robotics examples that the user can readily reproduce and then adapt, for fast prototyping and benchmarking. The code is available at https://github.com/CAOR-MINES-ParisTech/ukfm.
△ Less
Submitted 11 March, 2020; v1 submitted 3 February, 2020;
originally announced February 2020.
-
A New Approach to 3D ICP Covariance Estimation
Authors:
Martin Brossard,
Silvere Bonnabel,
Axel Barrau
Abstract:
In mobile robotics, scan matching of point clouds using Iterative Closest Point (ICP) allows estimating sensor displacements. It may prove important to assess the associated uncertainty about the obtained rigid transformation, especially for sensor fusion purposes. In this paper we propose a novel approach to 3D uncertainty of ICP that accounts for all the sources of error as listed in Censi's pio…
▽ More
In mobile robotics, scan matching of point clouds using Iterative Closest Point (ICP) allows estimating sensor displacements. It may prove important to assess the associated uncertainty about the obtained rigid transformation, especially for sensor fusion purposes. In this paper we propose a novel approach to 3D uncertainty of ICP that accounts for all the sources of error as listed in Censi's pioneering work [1], namely wrong convergence, underconstrained situations, and sensor noise. Our approach builds on two facts. First, the uncertainty about the ICP's output fully depends on the initialization accuracy. Thus speaking of the covariance of ICP makes sense only in relation to the initialization uncertainty, which generally stems from odometry errors. We capture this using the unscented transform, which also reflects correlations between initial and final uncertainties. Then, assuming white sensor noise leads to overoptimism as ICP is biased owing to e.g. calibration biases, which we account for. Our solution is tested on publicly available real data ranging from structured to unstructured environments, where our algorithm predicts consistent results with actual uncertainty, and compares favorably to previous methods.
△ Less
Submitted 27 January, 2020; v1 submitted 12 September, 2019;
originally announced September 2019.
-
AI-IMU Dead-Reckoning
Authors:
Martin Brossard,
Axel Barrau,
Silvère Bonnabel
Abstract:
In this paper we propose a novel accurate method for dead-reckoning of wheeled vehicles based only on an Inertial Measurement Unit (IMU). In the context of intelligent vehicles, robust and accurate dead-reckoning based on the IMU may prove useful to correlate feeds from imaging sensors, to safely navigate through obstructions, or for safe emergency stops in the extreme case of exteroceptive sensor…
▽ More
In this paper we propose a novel accurate method for dead-reckoning of wheeled vehicles based only on an Inertial Measurement Unit (IMU). In the context of intelligent vehicles, robust and accurate dead-reckoning based on the IMU may prove useful to correlate feeds from imaging sensors, to safely navigate through obstructions, or for safe emergency stops in the extreme case of exteroceptive sensors failure. The key components of the method are the Kalman filter and the use of deep neural networks to dynamically adapt the noise parameters of the filter. The method is tested on the KITTI odometry dataset, and our dead-reckoning inertial method based only on the IMU accurately estimates 3D position, velocity, orientation of the vehicle and self-calibrates the IMU biases. We achieve on average a 1.10% translational error and the algorithm competes with top-ranked methods which, by contrast, use LiDAR or stereo vision. We make our implementation open-source at: https://github.com/mbrossar/ai-imu-dr
△ Less
Submitted 12 April, 2019;
originally announced April 2019.
-
Exploiting Symmetries to Design EKFs with Consistency Properties for Navigation and SLAM
Authors:
Martin Brossard,
Axel Barrau,
Silvère Bonnabel
Abstract:
The Extended Kalman Filter (EKF) is both the historical algorithm for multi-sensor fusion and still state of the art in numerous industrial applications. However, it may prove inconsistent in the presence of unobservability under a group of transformations. In this paper we first build an alternative EKF based on an alternative nonlinear state error. This EKF is intimately related to the the…
▽ More
The Extended Kalman Filter (EKF) is both the historical algorithm for multi-sensor fusion and still state of the art in numerous industrial applications. However, it may prove inconsistent in the presence of unobservability under a group of transformations. In this paper we first build an alternative EKF based on an alternative nonlinear state error. This EKF is intimately related to the theory of the Invariant EKF (IEKF). Then, under a simple compatibility assumption between the error and the transformation group, we prove the linearized model of the alternative EKF automatically captures the unobservable directions, and many desirable properties of the linear case then directly follow. This provides a novel fundamental result in filtering theory. We apply the theory to multi-sensor fusion for navigation, when all the sensors are attached to the vehicle and do not have access to absolute information, as typically occurs in GPS-denied environments. In the context of Simultaneous Localization And Mapping (SLAM), Monte-Carlo runs and comparisons to OC-EKF, robocentric EKF, and optimization-based smoothing algorithms (iSAM) illustrate the results. The proposed EKF is also proved to outperform standard EKF and to achieve comparable performance to iSAM on a publicly available real dataset for multi-robot SLAM.
△ Less
Submitted 13 March, 2019;
originally announced March 2019.
-
RINS-W: Robust Inertial Navigation System on Wheels
Authors:
Martin Brossard,
Axel Barrau,
Silvere Bonnabel
Abstract:
This paper proposes a real-time approach for long-term inertial navigation based only on an Inertial Measurement Unit (IMU) for self-localizing wheeled robots. The approach builds upon two components: 1) a robust detector that uses recurrent deep neural networks to dynamically detect a variety of situations of interest, such as zero velocity or no lateral slip; and 2) a state-of-the-art Kalman fil…
▽ More
This paper proposes a real-time approach for long-term inertial navigation based only on an Inertial Measurement Unit (IMU) for self-localizing wheeled robots. The approach builds upon two components: 1) a robust detector that uses recurrent deep neural networks to dynamically detect a variety of situations of interest, such as zero velocity or no lateral slip; and 2) a state-of-the-art Kalman filter which incorporates this knowledge as pseudo-measurements for localization. Evaluations on a publicly available car dataset demonstrates that the proposed scheme may achieve a final precision of 20 m for a 21 km long trajectory of a vehicle driving for over an hour, equipped with an IMU of moderate precision (the gyro drift rate is 10 deg/h). To our knowledge, this is the first paper which combines sophisticated deep learning techniques with state-of-the-art filtering methods for pure inertial navigation on wheeled vehicles and as such opens up for novel data-driven inertial navigation techniques. Moreover, albeit taylored for IMU-only based localization, our method may be used as a component for self-localization of wheeled robots equipped with a more complete sensor suite.
△ Less
Submitted 28 February, 2020; v1 submitted 6 March, 2019;
originally announced March 2019.
-
Invariant Smoothing on Lie Groups
Authors:
Paul Chauchat,
Axel Barrau,
Silvère Bonnabel
Abstract:
In this paper we propose a (non-linear) smoothing algorithm for group-affine observation systems, a recently introduced class of estimation problems on Lie groups that bear a particular structure. As most non-linear smoothing methods, the proposed algorithm is based on a maximum a posteriori estimator, determined by optimization. But owing to the specific properties of the considered class of prob…
▽ More
In this paper we propose a (non-linear) smoothing algorithm for group-affine observation systems, a recently introduced class of estimation problems on Lie groups that bear a particular structure. As most non-linear smoothing methods, the proposed algorithm is based on a maximum a posteriori estimator, determined by optimization. But owing to the specific properties of the considered class of problems, the involved linearizations are proved to have a form of independence with respect to the current estimates, leveraged to avoid (partially or sometimes totally) the need to relinearize. The method is validated on a robot localization example, both in simulations and on real experimental data.
△ Less
Submitted 5 August, 2018; v1 submitted 6 March, 2018;
originally announced March 2018.
-
An EKF-SLAM algorithm with consistency properties
Authors:
Axel Barrau,
Silvere Bonnabel
Abstract:
In this paper we address the inconsistency of the EKF-based SLAM algorithm that stems from non-observability of the origin and orientation of the global reference frame. We prove on the non-linear two-dimensional problem with point landmarks observed that this type of inconsistency is remedied using the Invariant EKF, a recently introduced variant ot the EKF meant to account for the symmetries of…
▽ More
In this paper we address the inconsistency of the EKF-based SLAM algorithm that stems from non-observability of the origin and orientation of the global reference frame. We prove on the non-linear two-dimensional problem with point landmarks observed that this type of inconsistency is remedied using the Invariant EKF, a recently introduced variant ot the EKF meant to account for the symmetries of the state space. Extensive Monte-Carlo runs illustrate the theoretical results.
△ Less
Submitted 2 September, 2016; v1 submitted 21 October, 2015;
originally announced October 2015.
-
Invariant EKF Design for Scan Matching-aided Localization
Authors:
Martin Barczyk,
Silvère Bonnabel,
Jean-Emmanuel Deschaud,
François Goulette
Abstract:
Localization in indoor environments is a technique which estimates the robot's pose by fusing data from onboard motion sensors with readings of the environment, in our case obtained by scan matching point clouds captured by a low-cost Kinect depth camera. We develop both an Invariant Extended Kalman Filter (IEKF)-based and a Multiplicative Extended Kalman Filter (MEKF)-based solution to this probl…
▽ More
Localization in indoor environments is a technique which estimates the robot's pose by fusing data from onboard motion sensors with readings of the environment, in our case obtained by scan matching point clouds captured by a low-cost Kinect depth camera. We develop both an Invariant Extended Kalman Filter (IEKF)-based and a Multiplicative Extended Kalman Filter (MEKF)-based solution to this problem. The two designs are successfully validated in experiments and demonstrate the advantage of the IEKF design.
△ Less
Submitted 4 March, 2015;
originally announced March 2015.
-
On the Covariance of ICP-based Scan-matching Techniques
Authors:
Silvère Bonnabel,
Martin Barczyk,
François Goulette
Abstract:
This paper considers the problem of estimating the covariance of roto-translations computed by the Iterative Closest Point (ICP) algorithm. The problem is relevant for localization of mobile robots and vehicles equipped with depth-sensing cameras (e.g., Kinect) or Lidar (e.g., Velodyne). The closed-form formulas for covariance proposed in previous literature generally build upon the fact that the…
▽ More
This paper considers the problem of estimating the covariance of roto-translations computed by the Iterative Closest Point (ICP) algorithm. The problem is relevant for localization of mobile robots and vehicles equipped with depth-sensing cameras (e.g., Kinect) or Lidar (e.g., Velodyne). The closed-form formulas for covariance proposed in previous literature generally build upon the fact that the solution to ICP is obtained by minimizing a linear least-squares problem. In this paper, we show this approach needs caution because the rematching step of the algorithm is not explicitly accounted for, and applying it to the point-to-point version of ICP leads to completely erroneous covariances. We then provide a formal mathematical proof why the approach is valid in the point-to-plane version of ICP, which validates the intuition and experimental results of practitioners.
△ Less
Submitted 16 March, 2016; v1 submitted 15 October, 2014;
originally announced October 2014.
-
An Invariant Linear Quadratic Gaussian controller for a simplified car
Authors:
Sébastien Diemer,
Silvère Bonnabel
Abstract:
In this paper, we consider the problem of tracking a reference trajectory for a simplified car model based on unicycle kinematics, whose position only is measured, and where the control input and the measurements are corrupted by independent Gaussian noises. To tackle this problem we devise a novel observer-controller: the invariant Linear Quadratic Gaussian controller (ILQG). It is based on the L…
▽ More
In this paper, we consider the problem of tracking a reference trajectory for a simplified car model based on unicycle kinematics, whose position only is measured, and where the control input and the measurements are corrupted by independent Gaussian noises. To tackle this problem we devise a novel observer-controller: the invariant Linear Quadratic Gaussian controller (ILQG). It is based on the Linear Quadratic Gaussian controller, but the equations are slightly modified to account for, and to exploit, the symmetries of the problem. The gain tuning exhibits a reduced dependency on the estimated trajectory, and is thus less sensitive to misestimates. Beyond the fact the invariant approach is sensible (there is no reason why the controller performance should depend on whether the reference trajectory is heading west or south), we show through simulations that the ILQG outperforms the conventional LQG controller in case of large noises or large initial uncertainties. We show that those robustness properties may also prove useful for motion planning applications.
△ Less
Submitted 18 June, 2014; v1 submitted 6 June, 2014;
originally announced June 2014.
-
Experimental Implementation of an Invariant Extended Kalman Filter-based Scan Matching SLAM
Authors:
Martin Barczyk,
Silvère Bonnabel,
Jean-Emmanuel Deschaud,
François Goulette
Abstract:
We describe an application of the Invariant Extended Kalman Filter (IEKF) design methodology to the scan matching SLAM problem. We review the theoretical foundations of the IEKF and its practical interest of guaranteeing robustness to poor state estimates, then implement the filter on a wheeled robot hardware platform. The proposed design is successfully validated in experimental testing.
We describe an application of the Invariant Extended Kalman Filter (IEKF) design methodology to the scan matching SLAM problem. We review the theoretical foundations of the IEKF and its practical interest of guaranteeing robustness to poor state estimates, then implement the filter on a wheeled robot hardware platform. The proposed design is successfully validated in experimental testing.
△ Less
Submitted 20 March, 2014;
originally announced March 2014.
-
Priority-based intersection management with kinodynamic constraints
Authors:
Jean Gregoire,
Silvère Bonnabel,
Arnaud de La Fortelle
Abstract:
We consider the problem of coordinating a collection of robots at an intersection area taking into account dynamical constraints due to actuator limitations. We adopt the coordination space approach, which is standard in multiple robot motion planning. Assuming the priorities between robots are assigned in advance and the existence of a collision-free trajectory respecting those priorities, we pro…
▽ More
We consider the problem of coordinating a collection of robots at an intersection area taking into account dynamical constraints due to actuator limitations. We adopt the coordination space approach, which is standard in multiple robot motion planning. Assuming the priorities between robots are assigned in advance and the existence of a collision-free trajectory respecting those priorities, we propose a provably safe trajectory planner satisfying kinodynamic constraints. The algorithm is shown to run in real time and to return safe (collision-free) trajectories. Simulation results on synthetic data illustrate the benefits of the approach.
△ Less
Submitted 5 May, 2014; v1 submitted 22 October, 2013;
originally announced October 2013.
-
Intrinsic filtering on Lie groups with applications to attitude estimation
Authors:
Axel Barrau,
Silvere Bonnabel
Abstract:
This paper proposes a probabilistic approach to the problem of intrinsic filtering of a system on a matrix Lie group with invariance properties. The problem of an invariant continuous-time model with discrete-time measurements is cast into a rigorous stochastic and geometric framework. Building upon the theory of continuous-time invariant observers, we show that, as in the linear case, the error…
▽ More
This paper proposes a probabilistic approach to the problem of intrinsic filtering of a system on a matrix Lie group with invariance properties. The problem of an invariant continuous-time model with discrete-time measurements is cast into a rigorous stochastic and geometric framework. Building upon the theory of continuous-time invariant observers, we show that, as in the linear case, the error equation is a Markov chain that does not depend on the state estimate. Thus, when the filter's gains are held fixed, and the filter admits almost-global convergence properties with noise turned off, the noisy error's distribution is proved to converge to a stationary distribution, providing insight into the mathematical theory of filtering on Lie groups. For engineering purposes we also introduce the discrete-time Invariant Extended Kalman Filter, for which the trusted covariance matrix is shown to asymptotically converge, and some numerically more involved sample-based methods as well to compute the Kalman gains. The methods are applied to attitude estimation, allowing to derive novel theoretical results in this field, and illustrated through simulations on synthetic data.
△ Less
Submitted 12 September, 2014; v1 submitted 9 October, 2013;
originally announced October 2013.
-
Robust multirobot coordination using priority encoded homotopic constraints
Authors:
Jean Gregoire,
Silvère Bonnabel,
Arnaud de La Fortelle
Abstract:
We study the problem of coordinating multiple robots along fixed geometric paths. Our contribution is threefold. First we formalize the intuitive concept of priorities as a binary relation induced by a feasible coordination solution, without excluding the case of robots following each other on the same geometric path. Then we prove that two paths in the coordination space are continuously deformab…
▽ More
We study the problem of coordinating multiple robots along fixed geometric paths. Our contribution is threefold. First we formalize the intuitive concept of priorities as a binary relation induced by a feasible coordination solution, without excluding the case of robots following each other on the same geometric path. Then we prove that two paths in the coordination space are continuously deformable into each other if and only if they induce the \emph{same priority graph}, that is, the priority graph uniquely encodes homotopy classes of coordination solutions. Finally, we give a simple control law allowing to safely navigate into homotopy classes \emph{under kinodynamic constraints} even in the presence of unexpected events, such as a sudden robot deceleration without notice. It appears the freedom within homotopy classes allows to much deviate from any pre-planned trajectory without ever colliding nor having to re-plan the assigned priorities.
△ Less
Submitted 4 August, 2015; v1 submitted 4 June, 2013;
originally announced June 2013.
-
An anisotropy preserving metric for DTI processing
Authors:
Anne Collard,
Silvère Bonnabel,
Christophe Phillips,
Rodolphe Sepulchre
Abstract:
Statistical analysis of Diffusion Tensor Imaging (DTI) data requires a computational framework that is both numerically tractable (to account for the high dimensional nature of the data) and geometric (to account for the nonlinear nature of diffusion tensors). Building upon earlier studies that have shown that a Riemannian framework is appropriate to address these challenges, the present paper pro…
▽ More
Statistical analysis of Diffusion Tensor Imaging (DTI) data requires a computational framework that is both numerically tractable (to account for the high dimensional nature of the data) and geometric (to account for the nonlinear nature of diffusion tensors). Building upon earlier studies that have shown that a Riemannian framework is appropriate to address these challenges, the present paper proposes a novel metric and an accompanying computational framework for DTI data processing. The proposed metric retains the geometry and the computational tractability of earlier methods grounded in the affine invariant metric. In addition, and in contrast to earlier methods, it provides an interpolation method which preserves anisotropy, a central information carried by diffusion tensor data.
△ Less
Submitted 10 October, 2012;
originally announced October 2012.
-
Fixed-rank matrix factorizations and Riemannian low-rank optimization
Authors:
B. Mishra,
G. Meyer,
S. Bonnabel,
R. Sepulchre
Abstract:
Motivated by the problem of learning a linear regression model whose parameter is a large fixed-rank non-symmetric matrix, we consider the optimization of a smooth cost function defined on the set of fixed-rank matrices. We adopt the geometric framework of optimization on Riemannian quotient manifolds. We study the underlying geometries of several well-known fixed-rank matrix factorizations and th…
▽ More
Motivated by the problem of learning a linear regression model whose parameter is a large fixed-rank non-symmetric matrix, we consider the optimization of a smooth cost function defined on the set of fixed-rank matrices. We adopt the geometric framework of optimization on Riemannian quotient manifolds. We study the underlying geometries of several well-known fixed-rank matrix factorizations and then exploit the Riemannian quotient geometry of the search space in the design of a class of gradient descent and trust-region algorithms. The proposed algorithms generalize our previous results on fixed-rank symmetric positive semidefinite matrices, apply to a broad range of applications, scale to high-dimensional problems and confer a geometric basis to recent contributions on the learning of fixed-rank non-symmetric matrices. We make connections with existing algorithms in the context of low-rank matrix completion and discuss relative usefulness of the proposed framework. Numerical experiments suggest that the proposed algorithms compete with the state-of-the-art and that manifold optimization offers an effective and versatile framework for the design of machine learning algorithms that learn a fixed-rank matrix.
△ Less
Submitted 24 April, 2013; v1 submitted 3 September, 2012;
originally announced September 2012.
-
Accurate 3D maps from depth images and motion sensors via nonlinear Kalman filtering
Authors:
Thibault Hervier,
Silvère Bonnabel,
François Goulette
Abstract:
This paper investigates the use of depth images as localisation sensors for 3D map building. The localisation information is derived from the 3D data thanks to the ICP (Iterative Closest Point) algorithm. The covariance of the ICP, and thus of the localization error, is analysed, and described by a Fisher Information Matrix. It is advocated this error can be much reduced if the data is fused with…
▽ More
This paper investigates the use of depth images as localisation sensors for 3D map building. The localisation information is derived from the 3D data thanks to the ICP (Iterative Closest Point) algorithm. The covariance of the ICP, and thus of the localization error, is analysed, and described by a Fisher Information Matrix. It is advocated this error can be much reduced if the data is fused with measurements from other motion sensors, or even with prior knowledge on the motion. The data fusion is performed by a recently introduced specific extended Kalman filter, the so-called Invariant EKF, and is directly based on the estimated covariance of the ICP. The resulting filter is very natural, and is proved to possess strong properties. Experiments with a Kinect sensor and a three-axis gyroscope prove clear improvement in the accuracy of the localization, and thus in the accuracy of the built 3D map.
△ Less
Submitted 16 May, 2012;
originally announced May 2012.
-
Stochastic gradient descent on Riemannian manifolds
Authors:
Silvere Bonnabel
Abstract:
Stochastic gradient descent is a simple approach to find the local minima of a cost function whose evaluations are corrupted by noise. In this paper, we develop a procedure extending stochastic gradient descent algorithms to the case where the function is defined on a Riemannian manifold. We prove that, as in the Euclidian case, the gradient descent algorithm converges to a critical point of the c…
▽ More
Stochastic gradient descent is a simple approach to find the local minima of a cost function whose evaluations are corrupted by noise. In this paper, we develop a procedure extending stochastic gradient descent algorithms to the case where the function is defined on a Riemannian manifold. We prove that, as in the Euclidian case, the gradient descent algorithm converges to a critical point of the cost function. The algorithm has numerous potential applications, and is illustrated here by four examples. In particular a novel gossip algorithm on the set of covariance matrices is derived and tested numerically.
△ Less
Submitted 19 November, 2013; v1 submitted 22 November, 2011;
originally announced November 2011.
-
Symmetries in observer design: review of some recent results and applications to EKF-based SLAM
Authors:
Silvere Bonnabel
Abstract:
In this paper, we first review the theory of symmetry-preserving observers and we mention some recent results. Then, we apply the theory to Extended Kalman Filter-based Simultaneous Localization and Mapping (EKF SLAM). It allows to derive a new (symmetry-preserving) Extended Kalman Filter for the non-linear SLAM problem that possesses convergence properties. We also prove a special choice of the g…
▽ More
In this paper, we first review the theory of symmetry-preserving observers and we mention some recent results. Then, we apply the theory to Extended Kalman Filter-based Simultaneous Localization and Mapping (EKF SLAM). It allows to derive a new (symmetry-preserving) Extended Kalman Filter for the non-linear SLAM problem that possesses convergence properties. We also prove a special choice of the gains ensures global exponential convergence.
△ Less
Submitted 11 May, 2011;
originally announced May 2011.
-
Regression on fixed-rank positive semidefinite matrices: a Riemannian approach
Authors:
Gilles Meyer,
Silvere Bonnabel,
Rodolphe Sepulchre
Abstract:
The paper addresses the problem of learning a regression model parameterized by a fixed-rank positive semidefinite matrix. The focus is on the nonlinear nature of the search space and on scalability to high-dimensional problems. The mathematical developments rely on the theory of gradient descent algorithms adapted to the Riemannian geometry that underlies the set of fixed-rank positive semidefini…
▽ More
The paper addresses the problem of learning a regression model parameterized by a fixed-rank positive semidefinite matrix. The focus is on the nonlinear nature of the search space and on scalability to high-dimensional problems. The mathematical developments rely on the theory of gradient descent algorithms adapted to the Riemannian geometry that underlies the set of fixed-rank positive semidefinite matrices. In contrast with previous contributions in the literature, no restrictions are imposed on the range space of the learned matrix. The resulting algorithms maintain a linear complexity in the problem size and enjoy important invariance properties. We apply the proposed algorithms to the problem of learning a distance function parameterized by a positive semidefinite matrix. Good performance is observed on classical benchmarks.
△ Less
Submitted 31 January, 2011; v1 submitted 7 June, 2010;
originally announced June 2010.
