Decentralized Collaborative State Estimation for Aided Inertial Navigation

In this paper, we present a Quaternion-based Error-State Extended Kalman Filter (Q-ESEKF) based on IMU propagation with an extension for Collaborative State Estimation (CSE) and a communication complexity of $\mathcal{O}(1)$ (in terms of required communication links). Our approach combines a versatile filter formulation with the concept of CSE, allowing independent state estimation on each of the agents and at the same time leveraging and statistically maintaining interdependencies between agents, after joint measurements and communication (i.e. relative position measurements) occur. We discuss the development of the overall framework and the probabilistic (re-)initialization of the agent’s states upon initial or recurring joint observations. Our approach is evaluated in a simulation framework on two prominent benchmark datasets in 3D.

[1]  Stergios I. Roumeliotis,et al.  Decentralized multi-robot cooperative localization using covariance intersection , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[2]  Stergios I. Roumeliotis,et al.  Analysis of multirobot localization uncertainty propagation , 2003, Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453).

[3]  Sonia Martínez,et al.  A centralized-equivalent decentralized implementation of Extended Kalman Filters for cooperative localization , 2014, 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[4]  Stergios I. Roumeliotis,et al.  Observability-based consistent EKF estimators for multi-robot cooperative localization , 2011, Auton. Robots.

[5]  Stergios I. Roumeliotis,et al.  Performance analysis of multirobot Cooperative localization , 2006, IEEE Transactions on Robotics.

[6]  Ryan M. Eustice,et al.  Characterizing the Uncertainty of Jointly Distributed Poses in the Lie Algebra , 2019, IEEE Transactions on Robotics.

[7]  Gaurav S. Sukhatme,et al.  Self-calibrating multi-sensor fusion with probabilistic measurement validation for seamless sensor switching on a UAV , 2016, 2016 IEEE International Conference on Robotics and Automation (ICRA).

[8]  Joel A. Hesch,et al.  Cooperative vision-aided inertial navigation using overlapping views , 2012, 2012 IEEE International Conference on Robotics and Automation.

[9]  Roland Siegwart,et al.  Versatile distributed pose estimation and sensor self-calibration for an autonomous MAV , 2012, 2012 IEEE International Conference on Robotics and Automation.

[10]  Jörg Stückler,et al.  The TUM VI Benchmark for Evaluating Visual-Inertial Odometry , 2018, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

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

[12]  Keith Yu Kit Leung,et al.  The UTIAS multi-robot cooperative localization and mapping dataset , 2011, Int. J. Robotics Res..

[13]  Wolfram Burgard,et al.  Recursive Decentralized Collaborative Localization for Sparsely Communicating Robots , 2016, Robotics: Science and Systems.

[14]  Roland Siegwart,et al.  The EuRoC micro aerial vehicle datasets , 2016, Int. J. Robotics Res..

[15]  Margarita Chli,et al.  CVI-SLAM—Collaborative Visual-Inertial SLAM , 2018, IEEE Robotics and Automation Letters.

[16]  George K. I. Mann,et al.  Decentralized Cooperative Localization for Heterogeneous Multi-robot System Using Split Covariance Intersection Filter , 2014, 2014 Canadian Conference on Computer and Robot Vision.

[17]  Fawzi Nashashibi,et al.  Cooperative Multi-Vehicle Localization Using Split Covariance Intersection Filter , 2013, IEEE Intelligent Transportation Systems Magazine.

[18]  Wolfram Burgard,et al.  Recursive decentralized localization for multi-robot systems with asynchronous pairwise communication , 2018, Int. J. Robotics Res..

[19]  Ming Yang,et al.  Split Covariance Intersection Filter: Theory and Its Application to Vehicle Localization , 2013, IEEE Transactions on Intelligent Transportation Systems.

[20]  Julien Marzat,et al.  Collaborative localization and formation flying using distributed stereo-vision , 2016, 2016 IEEE International Conference on Robotics and Automation (ICRA).

[21]  Fawzi Nashashibi,et al.  Cooperative multi-vehicle localization using split covariance intersection filter , 2012, 2012 IEEE Intelligent Vehicles Symposium.

[22]  Jeffrey K. Uhlmann,et al.  A non-divergent estimation algorithm in the presence of unknown correlations , 1997, Proceedings of the 1997 American Control Conference (Cat. No.97CH36041).

[23]  Roland Siegwart,et al.  Collaborative 6DoF Relative Pose Estimation for Two UAVs with Overlapping Fields of View , 2018, 2018 IEEE International Conference on Robotics and Automation (ICRA).

[24]  Roland Brockers,et al.  Long-Duration Autonomy for Small Rotorcraft UAS Including Recharging , 2018, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[25]  Stergios I. Roumeliotis,et al.  Distributed multirobot localization , 2002, IEEE Trans. Robotics Autom..

[26]  Christian Bettstetter,et al.  Robots that Sync and Swarm: A Proof of Concept in ROS 2 , 2019, 2019 International Symposium on Multi-Robot and Multi-Agent Systems (MRS).

[27]  Keith Yu Kit Leung,et al.  Decentralized Localization of Sparsely-Communicating Robot Networks: A Centralized-Equivalent Approach , 2010, IEEE Transactions on Robotics.