Higher Order Nonlinear Complementary Filtering on Lie Groups

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.