Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds

TL;DR

This paper introduces a principled method for integrating manifold-structured quantities like 3D orientations into generic estimation algorithms by encapsulating manifold structure with specific operators, enabling broader applicability and software modularity.

Contribution

The paper proposes a formal encapsulation of manifold structures using two operators, facilitating the integration of non-vector space quantities into standard estimation algorithms.

Findings

Operators satisfy axiomatic properties for manifold encapsulation.

Demonstrated application in least-squares estimation.

Applied in the Unscented Kalman Filter with successful results.

Abstract

Common estimation algorithms, such as least squares estimation or the Kalman filter, operate on a state in a state space S that is represented as a real-valued vector. However, for many quantities, most notably orientations in 3D, S is not a vector space, but a so-called manifold, i.e. it behaves like a vector space locally but has a more complex global topological structure. For integrating these quantities, several ad-hoc approaches have been proposed. Here, we present a principled solution to this problem where the structure of the manifold S is encapsulated by two operators, state displacement [+]:S x R^n --> S and its inverse [-]: S x S --> R^n. These operators provide a local vector-space view \delta; --> x [+] \delta; around a given state x. Generic estimation algorithms can then work on the manifold S mainly by replacing +/- with [+]/[-] where appropriate. We analyze these operators axiomatically, and demonstrate their use in least-squares estimation and the Unscented Kalman Filter. Moreover, we exploit the idea of encapsulation from a software engineering perspective in the Manifold Toolkit, where the [+]/[-] operators mediate between a "flat-vector" view for the generic algorithm and a "named-members" view for the problem specific functions.