Particle filtering on the Euclidean group: framework and applications

Abstract

We address general filtering problems on the Euclidean group SE(3). We first generalize, to stochastic nonlinear systems evolving on SE(3), the particle filter of Liu and West(17) for simultaneous estimation of the state and covariance. The filter is constructed in a coordinate-invariant way, and explicitly takes into account the geometry of SE(3) and P(n), the space of symmetric positive definite matrices. Some basic results for bilinear systems on SE(3) with linear and quadratic measurements are also derived. Three examples-GPS attitude estimation, needle tip location, and vision-based robot end-effector pose estimation-are presented to illustrate the framework.