Feedback Particle Filter on Riemannian Manifolds and Matrix Lie Groups

This paper is concerned with the problem of continuous-time nonlinear filtering of stochastic processes evolving on connected Riemannian manifolds without boundary. 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 an intrinsic 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 manifold, and a numerical algorithm is described for this purpose. For the special case when the manifold is a matrix Lie group, explicit formulae for the filter are derived, using the matrix coordinates. Filters for two example problems are presented: the attitude estimation problem on SO(3) and the robot localization problem in SE(3) . Comparisons are also provided between the FPF and popular algorithms for attitude estimation, namely the multiplicative extended Kalman filter (EKF), the invariant EKF, the unscented quaternion estimator, the invariant ensemble Kalman filter, and the bootstrap particle filter. Numerical simulations are presented to illustrate these comparisons.