Relative localization with symmetry preserving observers

Symmetry preserving observer design is a recently developed approach for deriving estimators which exploits the invariant properties of nonlinear systems. Multi-robot localization is inherently a system operating on SE(3), thereby posing an interesting problem for application of the filter. This paper presents an invariant extended Kalman filter design for the problem of multi-robot relative localization in 2.5D, for application in ground and aerial mobile platforms. A detailed derivation of the invariant filter is presented with numerical results analyzing its performance against a traditional EKF approach to the problem. The tracking errors, stability to random initialization and robustness to changing noise characteristics are evaluated. The strong geometric basis of the filter results in linear Kalman like gain convergence behavior which is desirable for numerical stability and applicability as a low cost scheduled gain observer to the problem.