Search

In this paper, we introduce the Iterated Invariant Extended Kalman Filter (IIEKF), which is an invariant extended Kalman filter (IEKF) where the updated state in the light of the latest measurement is defined as a maximum a posteriori (MAP) estimate. Under some compatibility requirements on the output map, we prove strong mathematical guarantees which echo those of the Kalman filter in the linear case. We apply the technique to two problems: solving a system of equations on a Lie group, and a problem of engineering interest, namely ego-localization of the hook of a crane. The latter serves as a benchmarking example, where the IIEKF favorably compares to other filters.

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.

This paper presents a gradient-based method for on-the-fly optimization for both per-frame and per-frequency window length of the short-time Fourier transform (STFT), related to previous work in which we developed a differentiable version of STFT by making the window length a continuous parameter. The resulting differentiable adaptive STFT possesses commendable properties, such as the ability to adapt in the same time-frequency representation to both transient and stationary components, while being easily optimized by gradient descent. We validate the performance of our method in vibration analysis., Comment: Accepted for IEEE International Conference on Acoustics, Speech, and SIgnal Processing (ICASSP) 2023

In this paper, we propose a differentiable version of the short-time Fourier transform (STFT) that allows for gradient-based optimization of the hop length or the frame temporal position by making these parameters continuous. Our approach provides improved control over the temporal positioning of frames, as the continuous nature of the hop length allows for a more finely-tuned optimization. Furthermore, our contribution enables the use of optimization methods such as gradient descent, which are more computationally efficient than conventional discrete optimization methods. Our differentiable STFT can also be easily integrated into existing algorithms and neural networks. We present a simulated illustration to demonstrate the efficacy of our approach and to garner interest from the research community., Comment: Accepted for IEEE Statistical Signal Processing workshop 2023. IEEE, 2023

In this paper we provide novel closed-form expressions enabling differentiation of any scalar function of the Kalman filter's outputs with respect to all its tuning parameters and to the measurements. The approach differs from the previous well-known sensitivity equations in that it is based on a backward (matrix) gradient calculation, that leads to drastic reductions of the overall computational cost. It is our hope that practitioners seeking numerical efficiency and reliability will benefit from the concise and exact equations derived in this paper and the methods that build upon them. They may notably lead to speed-ups when interfacing a neural network with a Kalman filter., Comment: Submitted

In this paper, we revisit the use of spectrograms in neural networks, by making the window length a continuous parameter optimizable by gradient descent instead of an empirically tuned integer-valued hyperparameter. The contribution is mostly theoretical at this point, but plugging the modified STFT into any existing neural network is straightforward. We first define a differentiable version of the STFT in the case where local bins centers are fixed and independent of the window length parameter. We then discuss the more difficult case where the window length affects the position and number of bins. We illustrate the benefits of this new tool on an estimation and a classification problems, showing it can be of interest not only to neural networks but to any STFT-based signal processing algorithm., Comment: 8 pages, 8 figures, published as a conference paper at EUSIPCO 2022

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)., Comment: Pre-print submitted to CDC 2022

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., Comment: Published in IEEE Transactions on Automatic Control, 21 January 2022

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.

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.

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., Comment: Published in the proceedings of the IEEE International Conference on Robotics and Automation (ICRA) 2020

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., Comment: IEEE Robotics and Automation Letters, IEEE In press

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.

The mean shift algorithm is a popular way to find modes of some probability density functions taking a specific kernel-based shape, used for clustering or visual tracking. Since its introduction, it underwent several practical improvements and generalizations, as well as deep theoretical analysis mainly focused on its convergence properties. In spite of encouraging results, this question has not received a clear general answer yet. In this paper we focus on a specific class of kernels, adapted in particular to the distributions clustering applications which motivated this work. We show that a novel Mean Shift variant adapted to them can be derived, and proved to converge after a finite number of iterations. In order to situate this new class of methods in the general picture of the Mean Shift theory, we alo give a synthetic exposure of existing results of this field.

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., Comment: IEEE Robotics and Automation Letters, IEEE 2020

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

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.

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.

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., Comment: Accepted for publication at IROS 2018

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.

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., Comment: Submitted

In his 2005 paper, S.T. Smith proposed an intrinsic Cram\'er-Rao bound on the variance of estimators of a parameter defined on a Riemannian manifold. In the present technical note, we consider the special case where the parameter lives in a Lie group. In this case, by choosing, e.g., the right invariant metric, parallel transport becomes very simple, which allows a more straightforward and natural derivation of the bound in terms of Lie bracket, albeit for a slightly different definition of the estimation error. For bi-invariant metrics, the Lie group exponential map we use to define the estimation error, and the Riemannian exponential map used by S.T. Smith coincide, and we prove in this case that both results are identical indeed., Comment: To appear in the conference Geometric Sciences of Information GSI15

In this note an intrinsic version of the Cram\'er-Rao bound on estimation accuracy is established on the Special Orthogonal group $SO(3)$. It is intrinsic in the sense that it does not rely on a specific choice of coordinates on $SO(3)$: the result is derived using rotation matrices, but remains valid when using other parameterizations, such as quaternions. For any estimator $\hat R$ of $R\in SO(3)$ we give indeed a lower bound on the quantity $E(\log(R\hat R^T))$, that is, the estimation error expressed in terms of group multiplication, whereas the usual estimation error $E(\hat R-R)$ is meaningless on $SO(3)$. The result is first applied to Whaba's problem. Then, we consider the problem of a continuous-time nonlinear deterministic system on $SO(3)$ with discrete measurements subject to additive isotropic Gaussian noise, and we derive a lower bound to the estimation error covariance matrix. We prove the intrinsic Cram\'er-Rao bound coincides with the covariance matrix returned by the Invariant EKF, and thus can be computed online. This is in sharp contrast with the general case, where the bound can only be computed if the true trajectory of the system is known., Comment: To appear in the proceedings of IEEE Conference on Decision and Control 2015

We analyze the convergence aspects of the invariant extended Kalman filter (IEKF), when the latter is used as a deterministic non-linear observer on Lie groups, for continuous-time systems with discrete observations. One of the main features of invariant observers for left-invariant systems on Lie groups is that the estimation error is autonomous. In this paper we first generalize this result by characterizing the (much broader) class of systems for which this property holds. Then, we leverage the result to prove for those systems the local stability of the IEKF around any trajectory, under the standard conditions of the linear case. One mobile robotics example and one inertial navigation example illustrate the interest of the approach. Simulations evidence the fact that the EKF is capable of diverging in some challenging situations, where the IEKF with identical tuning keeps converging., Comment: This paper is going to be submitted for publication in IEEE Transactions on Automatic Control

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., Comment: Submitted