Silvere Bonnabel

Silvere Bonnabel, Rodolphe Sepulchre

This paper introduces a new metric and mean on the set of positive semidefinite matrices of fixed-rank. The proposed metric is derived from a well-chosen Riemannian quotient geometry that generalizes the reductive geometry of the positive cone and the associated natural metric. The resulting Riemannian space has strong geometrical properties: it is geodesically complete, and the metric is invariant with respect to all transformations that preserve angles (orthogonal transformations, scalings, and pseudoinversion). A meaningful approximation of the associated Riemannian distance is proposed, that can be efficiently numerically computed via a simple algorithm based on SVD. The induced mean preserves the rank, possesses the most desirable characteristics of a geometric mean, and is easy to compute.

Silvere Bonnabel, Rodolphe Sepulchre

An important property of the Kalman filter is that the underlying Riccati flow is a contraction for the natural metric of the cone of symmetric positive definite matrices. The present paper studies the geometry of a low-rank version of the Kalman filter. The underlying Riccati flow evolves on the manifold of fixed rank symmetric positive semidefinite matrices. Contraction properties of the low-rank flow are studied by means of a suitable metric recently introduced by the authors.

Silvere Bonnabel, Jean-Jacques Slotine

The contraction properties of the Extended Kalman Filter, viewed as a deterministic observer for nonlinear systems, are analyzed. This yields new conditions under which exponential convergence of the state error can be guaranteed. As contraction analysis studies the evolution of an infinitesimal discrepancy between neighboring trajectories, and thus stems from a differential framework, the sufficient convergence conditions are different from the ones that previously appeared in the literature, which were derived in a Lyapunov framework. This article sheds another light on the theoretical properties of this popular observer.

Silvere Bonnabel

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.

John Maidens, Axel Barrau, Silvere Bonnabel et al.

We present a method of exploiting symmetries of discrete-time optimal control problems to reduce the dimensionality of dynamic programming iterations. The results are derived for systems with continuous state variables, and can be applied to systems with continuous or discrete symmetry groups. We prove that symmetries of the state update equation and stage costs induce corresponding symmetries of the optimal cost function and the optimal policies. We then provide a general framework for computing the optimal cost function based on gridding a space of lower dimension than the original state space. This method does not require algebraic manipulation of the state update equations; it only requires knowledge of the symmetries that the state update equations possess. Since the method can be performed without any knowledge of the state update map beyond being able to evaluate it and verify its symmetries, this enables the method to be applied in a wide range of application problems. We illustrate these results on two six-dimensional optimal control problems that are computationally difficult to solve by dynamic programming without symmetry reduction.

Martin Brossard, Silvere Bonnabel, Axel Barrau

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.

Martin Brossard, Axel Barrau, Silvere Bonnabel

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.

Axel Barrau, Silvere Bonnabel

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.

Axel Barrau, Silvere Bonnabel

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.

Martin Brossard, Silvere Bonnabel, Axel Barrau

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.

Martin Brossard, Axel Barrau, Silvere Bonnabel

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.

Axel Barrau, Silvere Bonnabel

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.

Axel Barrau, Silvere Bonnabel

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.