Feedback Particle Filter on Matrix Lie Groups

This paper is concerned with the problem of continuous-time nonlinear filtering for stochastic processes on a compact and connected matrix Lie group without boundary, e.g. SO(n) and SE(n), in the presence of real-valued 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 for this problem. In its general form, the FPF provides a coordinate-free description of the filter that furthermore 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 Euclidean FPF. Specific examples for SO(2) and SO(3) are provided to help illustrate the filter using the phase and the quaternion coordinates, respectively.