Computationally Efficient Lie Algebraic Particle Filters for State Estimation

A set of particle filters on matrix Lie groups is presented for state estimation, where the particles live in Lie algebra. The dynamical equations in the Lie algebraic form and the log-linear property on the invariant error are introduced separately, to design two efficient particle time update schemes. All operations during the update are transferred to a vector space and at least only a single mean particle is propagated, the remaining particles are calculated by the error update on Lie algebra. This allows the tedious coordinate transformation in the original filter to be avoided, and significantly reduces the computational cost of numerical integration. Moreover, a detailed computational complexity analysis is provided, we compare scenarios in which these Lie algebraic particle filters can estimate efficiently, and discuss their respective drawbacks. The theoretical upper bounds of the relevant variables that can lead to efficiency improvements are also investigated. Finally, the experimental results show the effectiveness of the proposed Lie algebraic particle filters.