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.

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 $\pi$, VI aims at producing a simple but effective approximation $\hat \pi$ to $\pi$ 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 \pi$ 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 $\pi$ is log-concave.

The recently introduced matrix group$\mathbf {SE_2(3)}$provides a 5$\mathbf {\times }$5 matrix representation for the orientation, velocity, and position of an object in the 3-D space, a triplet we call “extended pose.” In this article, we build on this group to develop a theory to associate uncertainty with extended poses represented by 5$\mathbf {\times }$5 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 1-h-long experiments using our experimental car. It shows how handling rotating earth may be beneficial for long-term navigation within incremental smoothing algorithms.

An adaptive three-dimensional Kalman filter for the tracking of maneuvering targets in three dimensions is proposed. In the radar industry, numerous trackers are based on a constant velocity model, with a process noise covariance matrix Q which is adapted in real time to enhance tracking: it is kept at moderate values during straight lines where the constant velocity assumption applies and is increased during maneuvers. In the present paper we advocate a novel method to increase Q during maneuvers (and hence the Kalman gains) based on a recurrent neural network (RNN). The difficulty and the interest of our approach lies in the fact the neural network is trained together with the filter, by backpropagation through the filter, and hence learns the covariance matrix such as to directly maximize the accuracy of the final output.

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 SE2(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 SE2(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.

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.

We consider the problem of localization and navigation of Autonomous Underwater Vehicles (AUV) in the context of high performance subsea asset inspection missions in deep water. We propose a solution based on the recently introduced Unscented Kalman Filter on Manifolds (UKF-M) for onboard navigation to estimate the robot's location, attitude and velocity, using a precise round and rotating Earth navigation model. Our algorithm has the merit of seamlessly handling nonlinearity of attitude, and is far simpler to implement than the extended Kalman filter (EKF), which is widely used in the navigation industry. The unscented transform notably spares the user the computation of Jacobians and lends itself well to fast prototyping in the context of multi-sensor data fusion. Besides, we provide the community with feedback about implementation, and execution time is shown to be compatible with real-time. Realistic extensive Monte-Carlo simulations prove uncertainty is estimated with accuracy by the filter, and illustrate its convergence ability. Real experiments in the context of a 900m deep dive near Marseille (France) illustrate the relevance of the method.

The derivation of tight estimation lower bounds is a key player to design and assess the performance of new estimators. Considering a generic band-limited signal formulation and constant transmitter to receiver propagation delay, we propose a novel compact closed-form expression of the Cramér-Rao bound for time-delay estimation. This new formulation, especially easy to use, allows to derive the best (lowest) Cramér-Rao bound for a band-limited signal of given length and energy, which provides an estimation performance loss metric. These results are illustrated with two representative band-limited signals.

Odometry techniques are key to autonomous robot navigation, since they enable self-localization in the environment. However, designing a robust odometry system is particularly challenging when camera and LiDAR are uninformative or unavailable. In this paper, we leverage recent advances in deep learning and variational inference to correct dynamical and observation models for state-space systems. The methodology trains Gaussian processes on the residual between the original model and the ground truth, and is applied on publicly available datasets for robot navigation based on two wheel encoders, a fiber optic gyro, and an Inertial Measurement Unit (IMU). We also propose to build an Extended Kalman Filter (EKF) on the learned model using wheel speed sensors and the fiber optic gyro for state propagation, and the IMU to update the estimated state. Experimental results clearly demonstrate that the (learned) corrected models and EKF are more accurate than their original counterparts.

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 distance error 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.

Combining visual information with inertial measurements is a popular approach to achieve robust and autonomous navigation in robotics, specifically in GPS-denied environments. In this paper, building upon both the recent theory of Unscented Kalman Filtering on Lie Groups (UKF-LG) and more generally the theory of invariant Kalman filtering (IEKF), an innovative UKF is derived for the monocular visual simultaneous localization and mapping (SLAM) problem. The body pose, velocity, and the 3D landmarks' positions are viewed as a single element of a (high dimensional) Lie group SE2+p(3), which constitutes the state, and where the accelerometers' and gyrometers' biases are appended to the state and estimated as well. The efficiency of the approach is validated both on simulations and on five real datasets.

Fusing visual information with inertial measurements for state estimation has aroused major interests in recent years. However, combining a robust estimation with computational efficiency remains challenging, specifically for low-cost aerial vehicles in which the quality of the sensors and the processor power are constrained by size, weight and cost. In this paper, we present an innovative filter for stereo visual inertial odometry building on: (i) the recently introduced stereo multistate constraint Kalman filter; (ii) the invariant filtering theory; and (iii) the unscented Kalman filter (UKF) on Lie groups. Our solution combines accuracy, robustness and versatility of the UKF. We then compare our approach to state-of-art solutions in terms of accuracy, robustness and computational complexity on the EuRoC dataset and a challenging MAV outdoor dataset.

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.

In this paper, we first consider a simple Bayesian fusion problem in a matrix Lie group, and propose to tackle it using the unscented transform. The method is then leveraged to derive two simple alternative unscented Kalman filters on Lie groups, for both cases of noisy partial measurements of the state, and full state noisy measurements of the state on the group. The general method is applied to a robot localization problem, and results based on experimental data combined with extensive Monte-Carlo simulations at various noise levels illustrate the superiority of the approach over the standard UKF.

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.

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 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.