Search

We consider the problem of stochastic optimal control where the state-feedback control policies take the form of a probability distribution, and where a penalty on the entropy is added. By viewing the cost function as a Kullback-Leibler (KL) divergence between two Markov chains, we bring the tools from variational inference to bear on our optimal control problem. This allows for deriving a dynamic programming principle, where the value function is defined as a KL divergence again. We then resort to Gaussian distributions to approximate the control policies, and apply the theory to control affine nonlinear systems with quadratic costs. This results in closed-form recursive updates, which generalize LQR control and the backward Riccati equation. We illustrate this novel method on the simple problem of stabilizing an inverted pendulum.

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

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.

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

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

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

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., Comment: Final version published in Matrix Information Geometry, pp53-68, Springer Verlag, 2012

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., Comment: A slightly shorter version has been published in IEEE Transactions Automatic Control

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., Comment: This paper accompanies a presentation to be given at Eighth International Workshop on Robot Motion and Control (RoMoCo'11)

For linear time-invariant systems, a separation principle holds: stable observer and stable state feedback can be designed for the time-invariant system, and the combined observer and feedback will be stable. For non-linear systems, a local separation principle holds around steady-states, as the linearized system is time-invariant. This paper addresses the issue of a non-linear separation principle on Lie groups. For invariant systems on Lie groups, we prove there exists a large set of (time-varying) trajectories around which the linearized observer-controler system is time-invariant, as soon as a symmetry-preserving observer is used. Thus a separation principle holds around those trajectories. The theory is illustrated by a mobile robot example, and the developed ideas are then extended to a class of Lagrangian mechanical systems on Lie groups described by Euler-Poincare equations., Comment: Submitted to IFAC 2011

The generalization of the geometric mean of positive scalars to positive definite matrices has attracted considerable attention since the seminal work of Ando. The paper generalizes this framework of matrix means by proposing the definition of a rank-preserving mean for two or an arbitrary number of positive semi-definite matrices of fixed rank. The proposed mean is shown to be geometric in that it satisfies all the expected properties of a rank-preserving geometric mean. The work is motivated by operations on low-rank approximations of positive definite matrices in high-dimensional spaces., Comment: To appear in Linear Algebra and its Applications

Aghannan and Rouchon proposed a new design method of asymptotic observers for a class of nonlinear mechanical systems: Lagrangian systems with configuration (position) measurements. The observer is based on the Riemannian structure of the configuration manifold endowed with the kinetic energy metric and is intrinsic. They proved local convergence. When the system is conservative, we propose a globally convergent intrinsic reduced-observer based on the Jacobi metric. For non-conservative systems the observer can be used as a complement to the one of Aghannan and Rouchon. More generally the reduced-observer provides velocity estimation for geodesic flow with position measurements. Thus it can be (formally) used as a fluid flow soft sensor in the case of a perfect incompressible fluid. When the curvature is negative in all planes the geodesic flow is sensitive to initial conditions. Surprisingly this instability yields faster convergence., Comment: Published in IEEE Transactions on Automatic Control, Sept. 2010, Vol 55 Issue:9, pages : 2186 - 2191

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., Comment: the present version is very close to the published one. It contains some corrections with respect to the previous arxiv submssion

The present paper proposes a unified geometric framework for coordinated motion on Lie groups. It first gives a general problem formulation and analyzes ensuing conditions for coordinated motion. Then, it introduces a precise method to design control laws in fully actuated and underactuated settings with simple integrator dynamics. It thereby shows that coordination can be studied in a systematic way once the Lie group geometry of the configuration space is well characterized. This allows among others to retrieve control laws in the literature for particular examples. A link with Brockett's double bracket flows is also made. The concepts are illustrated on SO(3), SE(2) and SE(3)., Comment: Submitted