Feedback particle filter on matrix lie groups

Abstract

This paper is concerned with the problem of continuous-time nonlinear filtering of stochastic processes evolving on a compact and connected matrix Lie group without boundary, e.g. SO(n), in the presence of real-valued noisy observations. This problem is important to numerous applications in attitude estimation, visual tracking and robotic localization. The main contribution of this paper is to derive the feedback particle filter (FPF) algorithm as a solution for this problem. In its general form, the FPF provides a coordinate-free description of the filter that satisfies the geometric constraints of the manifold. The particle dynamics are encapsulated in a Stratonovich stochastic differential equation that preserves the feedback structure of the original FPF. Specific examples for SO(2) and SO(3) are provided to help illustrate the filter using the phase and the quaternion coordinates, respectively.