Feedback Particle Filter on Matrix Lie Groups

Abstract

This paper is concerned with the problem of continuous-time nonlinear filtering for stochastic processes on a connected matrix Lie group. The main contribution of this paper is to derive the feedback particle filter (FPF) algorithm for this problem. In its general form, the FPF is shown to provide a coordinate-free description of the filter that automatically satisfies the geometric constraints of the manifold. The particle dynamics are encapsulated in a Stratonovich stochastic differential equation that retains the feedback structure of the original (Euclidean) FPF. The implementation of the filter requires a solution of a Poisson equation on the Lie group, and two numerical algorithms are described for this purpose. As an example, the FPF is applied to the problem of attitude estimation - a nonlinear filtering problem on the Lie group SO(3). The formulae of the filter are described using both the rotation matrix and the quaternion coordinates. Comparisons are also provided between the FPF and some popular algorithms for attitude estimation, namely the multiplicative EKF, the unscented quaternion estimator, the left invariant EKF, and the invariant ensemble Kalman filter. Numerical simulations are presented to illustrate the comparisons.