An Optimal Control Approach to Particle Filtering on Lie Groups

Abstract

We study the filtering problem over a Lie group that plays an important role in robotics and aerospace applications. We present a new particle filtering algorithm based on stochastic control. In particular, our algorithm is based on a duality between smoothing and optimal control. Leveraging this duality, we reformulate the smoothing problem into an optimal control problem, and by approximately solving it (using, e.g., iLQR) we establish a superior proposal for particle smoothing. Combining it with a suitably designed sliding window mechanism, we obtain a particle filtering algorithm that suffers less from sample degeneracy compared with existing methods. The efficacy of our algorithm is illustrated by a filtering problem over SO(3) for satellite attitude estimation.