Feedback Particle Filter on Matrix Lie Groups

TL;DR

This paper develops a feedback particle filter for continuous-time nonlinear stochastic processes on matrix Lie groups, providing a geometric, coordinate-free approach with applications to attitude estimation.

Contribution

It derives a novel feedback particle filter algorithm tailored for matrix Lie groups, including implementation methods and comparisons with existing attitude estimation algorithms.

Findings

The FPF provides a coordinate-free, geometric filtering method on Lie groups.

Numerical algorithms for solving the Poisson equation on Lie groups are proposed.

The FPF outperforms some traditional attitude estimation algorithms in simulations.

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.