A Code for Unscented Kalman Filtering on Manifolds (UKF-M)

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.

[1]  Amit K. Sanyal,et al.  Unscented state estimation for rigid body attitude motion with a finite-time stable observer , 2016, 2016 IEEE 55th Conference on Decision and Control (CDC).

[2]  Axel Barrau,et al.  An EKF-SLAM algorithm with consistency properties , 2015, ArXiv.

[3]  Robert E. Mahony,et al.  A geometric nonlinear observer for simultaneous localisation and mapping , 2017, 2017 IEEE 56th Annual Conference on Decision and Control (CDC).

[4]  Axel Barrau,et al.  Intrinsic filtering on SO(3) with discrete-time observations , 2013, 52nd IEEE Conference on Decision and Control.

[5]  Timothy D. Barfoot,et al.  State Estimation for Robotics , 2017 .

[6]  Jean-Philippe Condomines,et al.  Nonlinear state estimation using an invariant unscented Kalman filter , 2013 .

[7]  Koushil Sreenath,et al.  Variation Based Extended Kalman Filter on $S^{2}$ , 2019, 2019 18th European Control Conference (ECC).

[8]  In Ho Choi,et al.  Features of Invariant Extended Kalman Filter Applied to Unmanned Aerial Vehicle Navigation , 2018, Sensors.

[9]  Nak Yong Ko,et al.  Improvement of Extended Kalman Filter Using Invariant Extended Kalman Filter , 2018, 2018 18th International Conference on Control, Automation and Systems (ICCAS).

[10]  Axel Barrau,et al.  The Invariant Extended Kalman Filter as a Stable Observer , 2014, IEEE Transactions on Automatic Control.

[11]  Martin Brossard,et al.  Unscented Kalman Filter on Lie Groups for Visual Inertial Odometry , 2018, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[12]  Udo Frese,et al.  Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds , 2011, Inf. Fusion.

[13]  Dinesh Atchuthan,et al.  A micro Lie theory for state estimation in robotics , 2018, ArXiv.

[14]  Vijay Kumar,et al.  Inertial Yaw-Independent Velocity and Attitude Estimation for High-Speed Quadrotor Flight , 2019, IEEE Robotics and Automation Letters.

[15]  James Richard Forbes,et al.  Continuous-time norm-constrained Kalman filtering , 2014, Autom..

[16]  P. Furgale,et al.  Pose estimation using linearized rotations and quaternion algebra , 2011 .

[17]  Henrique M. T. Menegaz,et al.  Unscented Kalman Filters for Riemannian State-Space Systems , 2018, IEEE Transactions on Automatic Control.

[18]  Xavier Pennec,et al.  Intrinsic Statistics on Riemannian Manifolds: Basic Tools for Geometric Measurements , 2006, Journal of Mathematical Imaging and Vision.

[19]  Audrey Giremus,et al.  Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions , 2014, Journal of Mathematical Imaging and Vision.

[20]  Chan Gook Park,et al.  Consistent EKF-Based Visual-Inertial Navigation Using Points and Lines , 2018, IEEE Sensors Journal.

[21]  Davide Scaramuzza,et al.  VIMO: Simultaneous Visual Inertial Model-Based Odometry and Force Estimation , 2019, IEEE Robotics and Automation Letters.

[22]  Jeffrey K. Uhlmann,et al.  Unscented filtering and nonlinear estimation , 2004, Proceedings of the IEEE.

[23]  Paul Timothy Furgale,et al.  Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems , 2014, IEEE Transactions on Robotics.

[24]  Gregory S. Chirikjian,et al.  Gaussian approximation of non-linear measurement models on Lie groups , 2014, 53rd IEEE Conference on Decision and Control.

[25]  Amit K. Sanyal,et al.  Unscented state estimation for rigid body motion on SE(3) , 2012, 2012 IEEE 51st IEEE Conference on Decision and Control (CDC).

[26]  Stergios I. Roumeliotis,et al.  A Quadratic-Complexity Observability-Constrained Unscented Kalman Filter for SLAM , 2013, IEEE Transactions on Robotics.

[27]  E. J. Lefferts,et al.  Kalman Filtering for Spacecraft Attitude Estimation , 1982 .

[28]  Gregory S. Chirikjian,et al.  Kinematic state estimation and motion planning for stochastic nonholonomic systems using the exponential map , 2008, Robotica.

[29]  E. Kraft,et al.  A quaternion-based unscented Kalman filter for orientation tracking , 2003, Sixth International Conference of Information Fusion, 2003. Proceedings of the.

[30]  Taeyoung Lee,et al.  Global unscented attitude estimation via the matrix Fisher distributions on SO(3) , 2016, 2016 American Control Conference (ACC).

[31]  J. S. Ortega.  Quaternion kinematics for the error-state KF , 2016 .

[32]  James Richard Forbes,et al.  Sigma Point Kalman Filtering on Matrix Lie Groups Applied to the SLAM Problem , 2017, GSI.

[33]  Jeffrey K. Uhlmann,et al.  New extension of the Kalman filter to nonlinear systems , 1997, Defense, Security, and Sensing.

[34]  Søren Hauberg,et al.  Unscented Kalman Filtering on Riemannian Manifolds , 2013, Journal of Mathematical Imaging and Vision.

[35]  Audrey Giremus,et al.  Discrete Extended Kalman Filter on Lie groups , 2013, 21st European Signal Processing Conference (EUSIPCO 2013).

[36]  F. Markley,et al.  Unscented Filtering for Spacecraft Attitude Estimation , 2003 .

[37]  Gregory S. Chirikjian,et al.  Error propagation on the Euclidean group with applications to manipulator kinematics , 2006, IEEE Transactions on Robotics.

[38]  G. Chirikjian.  Stochastic Models, Information Theory, and Lie Groups, Volume 1 , 2009 .

[39]  Gamini Dissanayake,et al.  Convergence and Consistency Analysis for a 3-D Invariant-EKF SLAM , 2017, IEEE Robotics and Automation Letters.

[40]  Levent Tunçel,et al.  Optimization algorithms on matrix manifolds , 2009, Math. Comput..

[41]  G. Chirikjian.  Stochastic models, information theory, and lie groups , 2012 .

[42]  Axel Barrau,et al.  Intrinsic Filtering on Lie Groups With Applications to Attitude Estimation , 2013, IEEE Transactions on Automatic Control.

[43]  Vijay Kumar,et al.  Visual inertial odometry for quadrotors on SE(3) , 2016, 2016 IEEE International Conference on Robotics and Automation (ICRA).

[44]  John Milnor,et al.  Analytic Proofs of the “Hairy Ball Theorem” and the Brouwer Fixed Point Theorem , 1978 .

[45]  Joan Solà,et al.  Quaternion kinematics for the error-state Kalman filter , 2015, ArXiv.

[46]  Miaomiao Wang,et al.  A Globally Exponentially Stable Nonlinear Hybrid Observer for 3D Inertial Navigation , 2018, 2018 IEEE Conference on Decision and Control (CDC).

[47]  Andreas Geiger,et al.  Vision meets robotics: The KITTI dataset , 2013, Int. J. Robotics Res..

[48]  Silvere Bonnabel.  Symmetries in observer design: review of some recent results and applications to EKF-based SLAM , 2011, ArXiv.

[49]  Silvere Bonnabel,et al.  Left-invariant extended Kalman filter and attitude estimation , 2007, 2007 46th IEEE Conference on Decision and Control.

[50]  Maani Ghaffari Jadidi,et al.  Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation , 2018, Robotics: Science and Systems.

[51]  Jean-Philippe Condomines,et al.  Pi-Invariant Unscented Kalman Filter for sensor fusion , 2014, 53rd IEEE Conference on Decision and Control.

[52]  Gamini Dissanayake,et al.  An invariant-EKF VINS algorithm for improving consistency , 2017, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[53]  Axel Barrau,et al.  Non-linear state error based extended Kalman filters with applications to navigation. (Filtres de Kalman étendus reposant sur une variable d'erreur non linéaire avec applications à la navigation) , 2015 .

[54]  Chan Gook Park,et al.  Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group , 2018, IEEE Sensors Journal.

[55]  Jean-Philippe Condomines,et al.  Unscented Kalman filtering on Lie groups , 2017, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[56]  Axel Barrau,et al.  Invariant Kalman Filtering , 2018, Annu. Rev. Control. Robotics Auton. Syst..

[57]  Martin Brossard,et al.  Exploiting Symmetries to Design EKFs With Consistency Properties for Navigation and SLAM , 2019, IEEE Sensors Journal.

[58]  Miaomiao Wang,et al.  Geometric Nonlinear Observer Design for SLAM on a Matrix Lie Group , 2018, 2018 IEEE Conference on Decision and Control (CDC).