Second-order-optimal filters on Lie groups

Abstract

We provide an explicit formula for the secondorder-optimal nonlinear filter for state estimation of systems on general Lie groups with disturbed measurements of inputs and outputs. Optimality is with respect to a deterministic cost measuring the cumulative energy in the unknown system disturbances (minimum-energy filtering). We show that the resulting filter will depend on the choice of affine connection, thus encoding the nonlinear geometry of the state space. For the case of attitude estimation, where we are given a second order (dynamic) mechanical system on the tangent bundle of the special orthogonal group SO(3), and where we choose the symmetric Cartan-Schouten (0)-connection, the resulting filter has the familiar form of a gradient observer combined with a matrix Riccati differential equation that updates the filter gain.

Second-Order-Optimal Minimum-Energy Filters on Lie Groups

“…We choose the usual symmetric Cartan-Schouten (0)-connection and the Cartan-Schouten (-)-connection on SO(3) for illustration, but different choices would be possible, resulting in different gain equations. To the best of our knowledge, this is the first such filter published for a (second-order) mechanical system, except for the conference version of this paper [11].…”

Second-Order Recursive Filtering on the Rigid-Motion Lie Group $${\text {SE}}_{3}$$ SE 3 Based on Nonlinear Observations

“…Our contributions reported in this paper amount to generalize the constant camera velocity model from [9] (non-linear measurement model) to polynomial models, in particular the constant acceleration model; to provide a complete derivation of the second-order minimum energy filter [41] as applied to camera motion estimation together with robust numerics that are consistent with the geometry and the structure of matrix Riccati equations; to report experiments demonstrating that higherorder kinematic models are more accurate than the constant velocity model [9] on synthetic (with kinematic camera tracks) and real world data and that they enable to reconstruct higher-order information; to report experiments comparing our approach to state-of-the-art extended Kalman Filters on Lie groups [12], indicating that our method is superior in coping with non-linearities of the observation function as well as in being more robust against imperfect initializations.…”

“…The rigorous way to describe kinematics is to use the tangent map (cf. [41]) of the left translation which is given by the following proposition:…”

“…where S 1 , S 2 ∈ R 6×6 and Q ∈ R 2×2 are symmetric, positive definite weighting matrices. From [41] we adopt the idea of a decay rate α > 0, and thus we introduce the weighting factor e −α(t−t0) on the right-hand side of (18):…”

“…[5]). Instead we follow the approach of Saccon et al [41] who derived a left-trivialized optimal Hamiltonian based on control theory on Lie groups [26]. This left-trivialized optimal Hamiltonian is defined byH − :…”