arXiv subjects

Silvère Bonnabel

Publications and source records attributed to Silvère Bonnabel.

31 records · Page 2Linked to original sources

arXiv2016

On the Covariance of ICP-based Scan-matching Techniques

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.

cs.CV↗

arXiv2015

The invariant extended Kalman filter as a stable observer

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.

eess.SY↗

arXiv2015

An intrinsic Cramér-Rao bound on SO(3) for (dynamic) attitude filtering

In this note an intrinsic version of the Cramér-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ér-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.

math.OC↗

arXiv2015

An intrinsic Cramér-Rao bound on Lie groups

In his 2005 paper, S.T. Smith proposed an intrinsic Cramér-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.

eess.SY↗

arXiv2015

Robust multirobot coordination using priority encoded homotopic constraints

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.

cs.RO↗

arXiv2015

Invariant EKF Design for Scan Matching-aided Localization

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.

eess.SY↗

arXiv2014

An Invariant Linear Quadratic Gaussian controller for a simplified car

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.

cs.RO↗

arXiv2014

Priority-based intersection management with kinodynamic constraints

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.

cs.RO↗

arXiv2014

Experimental Implementation of an Invariant Extended Kalman Filter-based Scan Matching SLAM

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.

eess.SY↗

arXiv2013

Optimal cooperative motion planning for vehicles at intersections

We consider the problem of cooperative intersection management. It arises in automated transportation systems for people or goods but also in multi-robots environment. Therefore many solutions have been proposed to avoid collisions. The main problem is to determine collision-free but also deadlock-free and optimal algorithms. Even with a simple definition of optimality, finding a global optimum is a problem of high complexity, especially for open systems involving a large and varying number of vehicles. This paper advocates the use of a mathematical framework based on a decomposition of the problem into a continuous optimization part and a scheduling problem. The paper emphasizes connections between the usual notion of vehicle priority and an abstract formulation of the scheduling problem in the coordination space. A constructive locally optimal algorithm is proposed. More generally, this work opens up for new computationally efficient cooperative motion planning algorithms.

eess.SY↗

arXiv2012

An anisotropy preserving metric for DTI processing

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.

cs.CV↗

arXiv2012

Accurate 3D maps from depth images and motion sensors via nonlinear Kalman filtering

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.

cs.RO↗

arXiv2008

Coordinated motion design on Lie groups

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

math.OC↗