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