A Nonlinear Filter for Pose Estimation Based on Fast Unscented Transform on Lie Groups

Abstract

This article presents a nonlinear estimator on matrix Lie group that performs a fast unscented transformation with natural evolution of sigma points from a geometric perspective. Different from the existing methods, the proposed method preserves the original dynamic equations on the manifold, which greatly reduces the computational time without changing the system configuration space or reducing the number of sigma points. We provide a new state propagation and update method of UKF on manifolds, where only the mean state is involved, and the remaining sigma points are calculated and propagated as incremental information based on the state of the previous step, according to the fundamental property of geometric filtering on the Lie group. Moreover, by decoupling the parameter variables, we investigate the upper limit of the efficiency improvement of the proposed algorithm compared to the traditional unscented transformation in different situations. Finally, two representative experiments are conducted to validate the proposed theory, the experiments show that the proposed method achieves desirable performance with much higher computational efficiency as compared with the existing UKF algorithms on manifolds.