Estimation de paramètres évoluant sur des groupes de Lie : application à la cartographie et à la localisation d'une caméra monoculaire

In this thesis, we derive novel parameter estimation algorithmsdedicated to parameters evolving on Lie groups. These algorithms are casted in aBayesian formalism, which allows us to establish a notion of uncertainty for theestimated parameters. To do so, a generalization of the multivariate normaldistribution to Lie groups, called concentrated normal distribution on Lie groups, isemployed.In a first part, we generalize the Continuous-Discrete Extended Kalman Filter (CDEKF),as well as the Discrete Extended Kalman Filter (D-EKF), to the case where thestate and the observations evolve on Lie groups. We obtain two novel algorithmscalled Continuous-Discrete Extended Kalman Filter on Lie Groups (CD-LG-EKF) andDiscrete Extended Kalman Filter on Lie Groups (D-LG-EKF).In a second part, we focus on bridging the gap between the formulation of intrinsicnon linear least squares criteria and Kalman filtering/smoothing on Lie groups. Wepropose a generalization of the Euclidean Iterated Extended Kalman Filter (IEKF) toLie groups, called LG-IEKF. We also derive a generalization of the Rauch-Tung-Striebel smoother (RTS), also known as Extended Kalman Smoother, to Lie groups,called LG-RTS.Finally, the concepts and algorithms presented in the thesis are employed in a seriesof applications. Firstly, we propose a novel simultaneous localization and mappingapproach. Secondly we develop an indoor camera localization framework. For thislatter purpose, we derived a novel Rao-Blackwellized particle smoother on Liegroups, which builds upon the LG-IEKF and the LG-RTS.Dans ce travail de thèse, nous proposons plusieurs algorithmespermettant d'estimer des paramètres évoluant sur des groupes de Lie. Cesalgorithmes s’inscrivent de manière générale dans un cadre bayésien, ce qui nouspermet d'établir une notion d'incertitude sur les paramètres estimés. Pour ce faire,nous utilisons une généralisation de la distribution normale multivariée aux groupesde Lie, appelée distribution normale concentrée sur groupe de Lie.Dans une première partie, nous nous intéressons au problème du filtrage de Kalmanà temps discret et continu-discret où l’état et les d’observations appartiennent à desgroupes de Lie. Cette étude nous conduit à la proposition de deux filtres ; le CD-LGEKFqui permet de résoudre un problème à temps continu-discret et le D-LG-EKF quipermet de résoudre un problème à temps discret.Dans une deuxième partie, nous nous inspirons du lien entre optimisation et filtragede Kalman, qui a conduit au développement du filtrage de Kalman étendu itéré surespace euclidien, en le transposant aux groupes de Lie. Nous montrons ainsicomment obtenir une généralisation du filtre de Kalman étendu itéré aux groupes deLie, appelée LG-IEKF, ainsi qu’une généralisation du lisseur de Rauch-Tung-Striebelaux groupes de Lie, appelée LG-RTS.Finalement, dans une dernière partie, les concepts et algorithmes d’estimation surgroupes de Lie proposés dans la thèse sont utilisés dans le but de fournir dessolutions au problème de la cartographie d'un environnement à partir d'une caméramonoculaire d'une part, et au problème de la localisation d'une caméra monoculairese déplaçant dans un environnement préalablement cartographié d'autre part

A Quick Guide for the Iterated Extended Kalman Filter on Manifolds

The extended Kalman filter (EKF) is a common state estimation method for discrete nonlinear systems. It recursively executes the propagation step as time goes by and the update step when a set of measurements arrives. In the update step, the EKF linearizes the measurement function only once. In contrast, the iterated EKF (IEKF) refines the state in the update step by iteratively solving a least squares problem. The IEKF has been extended to work with state variables on manifolds which have differentiable $\boxplus$ and $\boxminus$ operators, including Lie groups. However, existing descriptions are often long, deep, and even with errors. This note provides a quick reference for the IEKF on manifolds, using freshman-level matrix calculus. Besides the bare-bone equations, we highlight the key steps in deriving them.Comment: 2 pages excluding reference

Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation

This paper derives a contact-aided inertial navigation observer for a 3D bipedal robot using the theory of invariant observer design. Aided inertial navigation is fundamentally a nonlinear observer design problem; thus, current solutions are based on approximations of the system dynamics, such as an Extended Kalman Filter (EKF), which uses a system's Jacobian linearization along the current best estimate of its trajectory. On the basis of the theory of invariant observer design by Barrau and Bonnabel, and in particular, the Invariant EKF (InEKF), we show that the error dynamics of the point contact-inertial system follows a log-linear autonomous differential equation; hence, the observable state variables can be rendered convergent with a domain of attraction that is independent of the system's trajectory. Due to the log-linear form of the error dynamics, it is not necessary to perform a nonlinear observability analysis to show that when using an Inertial Measurement Unit (IMU) and contact sensors, the absolute position of the robot and a rotation about the gravity vector (yaw) are unobservable. We further augment the state of the developed InEKF with IMU biases, as the online estimation of these parameters has a crucial impact on system performance. We evaluate the convergence of the proposed system with the commonly used quaternion-based EKF observer using a Monte-Carlo simulation. In addition, our experimental evaluation using a Cassie-series bipedal robot shows that the contact-aided InEKF provides better performance in comparison with the quaternion-based EKF as a result of exploiting symmetries present in the system dynamics.Comment: Published in the proceedings of Robotics: Science and Systems 201