Higher-Order Nonlinear Complementary Filtering on Lie Groups

TL;DR

This paper introduces a higher-order nonlinear complementary filtering method on Lie groups, extending linear filters to nonlinear systems for improved bias and noise filtering in pose estimation.

Contribution

It develops a novel higher-order nonlinear observer on Lie groups using a linear system for innovation, enhancing filtering flexibility and stability in nonlinear state estimation.

Findings

Demonstrates local asymptotic stability of the observer.

Shows improved filtering of bias and noise over specific bandwidths.

Provides a numerical example in pose estimation context.

Abstract

Nonlinear observer design for systems whose state space evolves on Lie groups is considered. The proposed method is similar to previously developed nonlinear observers in that it involves propagating the state estimate using a process model and corrects the propagated state estimate using an innovation term on the tangent space of the Lie group. In the proposed method, the innovation term is constructed by passing the gradient of an invariant cost function, resolved in a basis of the tangent space, through a linear time-invariant system. The introduction of the linear system completes the extension of linear complementary filters to nonlinear Lie group observers by allowing higher-order filtering. In practice, the proposed method allows for greater design freedom and, with the appropriate selection of the linear filter, the ability to filter bias and noise over specific bandwidths. A disturbance observer that accounts for constant and harmonic disturbances in group velocity measurements is also considered. Local asymptotic stability about the desired equilibrium point is demonstrated. A numerical example that demonstrates the desirable properties of the observer is presented in the context of pose estimation.