Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds

Common estimation algorithms, such as least squares estimation or the Kalman filter, operate on a state in a state space SS that is represented as a real-valued vector. However, for many quantities, most notably orientations in 3D, SS 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 SS is encapsulated by two operators, state displacement :S×Rn→S:S×Rn→S and its inverse :S×S→Rn:S×S→Rn. These operators provide a local vector-space view δ ↦ x  δ around a given state x  . Generic estimation algorithms can then work on the manifold SS 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.

► Spaces of orientations cause difficulties in sensor fusion algorithms. ► We present a principled and generic solution based on manifolds. ► This solution is axiomatically encapsulated to allow for well-defined interfaces. ► Experiments demonstrate the superiority of manifold-based state representations.