Gaussian approximation of non-linear measurement models on Lie groups

Extended Kalman filters on Lie groups arise naturally in the context of pose estimation and more generally in robot localization and mapping. Typically in such settings one deals with nonlinear measurement models that are handled through linearization and linearized uncertainty transformation. To circumvent the loss of accuracy resulting from the typical coordinate-based linearization, this paper develops a method for accurately describing the probability density associated with nonlinear measurement models by a second-order approximation of a distribution defined directly on the Lie group configuration space. We show that, like the case of linearized measurement models, this density can be described well as a Gaussian distribution in exponential coordinates (though with different mean and covariance than those that result from linearized measurement models). And therefore previously developed methods for propagation of uncertainty and fusion of measurements can be applied to this generalized formulation without the a priori assumption of linearized measurement. A case study using a range-bearing model in planar robot localization is presented to demonstrate the method.