PaperlayerView exact revision on arXiv

Source-linked AI summary

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

Christoph Hertzberg, René Wagner, Udo Frese, Lutz Schröder

arXiv:1107.1119v1cs.ROcs.CVcs.MS

TL;DR

The paper addresses the mismatch between vector-based estimation algorithms and non-Euclidean state spaces such as 3D orientations. It introduces encapsulation operators that provide a local vector-space view of manifold states, analyzes them axiomatically, and applies them to least-squares estimation and the Unscented Kalman Filter. The paper also presents a toolkit supporting this separation, while existing quaternion and overparameterized approaches retain covariance or computational limitations.

  • Problem

    Vector-based sensor fusion algorithms do not directly support mathematically sound manifold representations, while ad-hoc orientation approaches can suffer singularities, constraints, or representation-specific implementation problems.

  • Method

    The paper defines ⊞ and ⊟ operators that map local manifold perturbations to Rn and mediate between generic algorithms and problem-specific state representations.

  • Results

    The authors analyze the operators axiomatically and demonstrate their use in least-squares estimation and the Unscented Kalman Filter, including practical evaluations where Euler-angle and scaled-axis representations fail at singularities.

  • Takeaways & Limitations

    ⊞-manifolds provide a principled local vector-space view that lets generic sensor-fusion algorithms operate on manifold-valued states while preserving representation-specific handling.

  • Takeaways & Limitations

    Overparameterized state representations can increase state dimension and computation time, while quaternion normalization can make covariance singular and create unknown overconfidence risks after nonlinear updates.

Abstract

from arXiv · show

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 δ; --> x [+] δ; 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.

1. Introduction

Sensor fusion must combine faithful state representations with algorithms designed for real-valued vectors, a mismatch that is especially problematic for manifold-valued quantities such as 3D orientations. The paper addresses this by encapsulating local manifold mappings in ⊞ and ⊟ operators so generic algorithms can operate without representation-specific modifications.

  • Sensor fusion combines information from multiple sensors into a joint belief while requiring state representations that describe reality and support meaningful algorithmic results.
  • Standard vector representations conflict with mathematically sound manifold representations, notably SO(3) for three-dimensional orientations.Minimal parameterizations can have singularities, while non-minimal representations introduce constraints.
  • Both minimal and non-minimal orientation approaches require representation-specific algorithm changes, preventing sensor fusion algorithms from remaining generic black boxes.
  • The proposed manifold view exploits the locality of sensor-fusion operations by mapping neighborhoods of an arbitrary manifold bidirectionally to Rn.For S2, the local neighborhood is mapped to a plane so standard algorithms can operate without explicitly encoding global topology.
  • The ⊞ operator applies a small Rn perturbation to a manifold state, while ⊟ computes the mapped difference between two states.
  • Replacing vector addition and subtraction with ⊞ and ⊟ lets generic algorithms use flattened perturbations while problem-specific functions retain natural state representations.The operators mediate between the generic algorithm’s flat-vector view and application code’s representation-specific view.

2. Related Work

Existing manifold-handling strategies modify generic estimation algorithms in representation-specific ways, introducing singularities, constraints, redundancy, or limited distributional flexibility. The paper frames the ⊞/⊟ approach as an axiomatized, more generic alternative that preserves manifold structure while adapting established algorithms.

  • Minimal parameterizations: Minimal parameterizations such as Euler angles can place singularities in the workspace, requiring recovery strategies or switching between alternative parameterizations.These singularities can make small state changes require very large parameter changes.
  • Overparameterizations: Overparameterized representations such as quaternions require normalization constraints, which can counteract optimization updates, complicate equations, or increase computation.Adding normalization as a measurement is only approximate unless its uncertainty is zero; retaining redundant dimensions also increases computation time.
  • Local manifold methods: Reference-state methods apply minimal perturbations locally and are practically similar to the ⊞-method, while Lie-group methods are equivalent for Lie groups such as SO(3).For Lie groups, the correspondence is s ⊞ δ = s · exp δ.
  • The ⊞/⊟ framework: The paper’s contribution is an explicit axiomatization of ⊞/⊟ operators that adapts R^n algorithms to general manifolds, including non-Lie-group manifolds such as S^2.The operators encapsulate the representation-specific operations while preserving generic algorithm structure.
  • Overparameterizations: Quaternion normalization in EKF-style methods can make covariance singular and may create overconfidence after nonlinear updates, despite the innovation covariance remaining positive definite.The paper states that the resulting zero-uncertainty degree of freedom could affect another degree of freedom after a nonlinear update.
  • Distributions on unit spheres: Sphere-specific distributions can represent manifold-valued uncertainty, but von Mises-Fisher distributions model only isotropic covariance and manifold distributions are difficult to combine with ordinary normal distributions.The paper contrasts these limitations with adapting established estimation algorithms through its approach.

3. The ⊞-Method

The ⊞-method encapsulates manifold structure through operators that provide a local vectorized view, allowing generic estimation algorithms to operate without knowing the state space’s internal representation.

  • State representations: The method replaces lossy Euclidean state translations with mathematically sound manifold representations, including compound states and representations of SO(3).For 3D orientations, quaternions or rotation matrices can represent SO(3), with matching representation-specific ⊞ and ⊟ operators.
  • Encapsulation operators: The ⊞-method uses ⊞ to add local perturbations to manifold states and ⊟ to recover perturbations between states.For y = x ⊞ δ, δ ∈ R^n is a perturbation at x; conversely, y ⊟ x returns the perturbation producing y from x.
  • Axiomatic structure: A ⊞-manifold is defined by a state space, the ⊞ and ⊟ operators, and a neighborhood of zero satisfying smoothness and axiomatic conditions.The axioms include a neutral perturbation, mutual recovery of states and perturbations, local uniqueness, and a distance inequality.
  • Encapsulation operators: The operators let generic algorithms compare and modify manifold states as if they were flat vectors while treating the state space as a black box.Problem-specific functions can use the state’s natural representation, while ⊞ and ⊟ translate between that view and the algorithm’s perturbation-vector view.
  • Probability distributions: The paper extends Gaussian distributions and related probabilistic concepts from vector spaces to ⊞-manifolds.A manifold-valued random variable can be constructed by applying ⊞ to a reference state and an R^n-valued random variable.
  • Axiomatic structure: Axiom (11d) requires distances between displaced states to be no greater than distances between their perturbation vectors, making the parameterization 1-Lipschitz.The paper identifies this condition as necessary for the manifold probability concepts developed in the subsection.

4. Least Squares Optimization and Kalman Filtering on ⊞-Manifolds

Least-squares optimization and the UKF can be adapted to ⊞-manifolds by performing state updates and comparisons with ⊞ and ⊟, with additional UKF changes for manifold means and covariances.

  • Overview: The ⊞-method adapts generic least-squares algorithms and the Unscented Kalman Filter to operate on ⊞-manifolds.The main substitution is replacing vector addition and subtraction with ⊞ and ⊟; the UKF also requires modified mean and covariance calculations.
  • Least Squares Optimization: In manifold least squares, state perturbations use ⊞ and measurement-space comparisons use ⊟, while the ⊟z term remains in finite-difference Jacobian calculations.The equivalence marked by ∗ in Table 1 holds only approximately.
  • Least Squares Optimization: For a measurement function between manifolds, the residual f(X) ⊟ z is modeled as Gaussian and yields a maximum likelihood solution.The paper states this result for f(X) ⊟ z ∼ N(0, Σ).
  • Least Squares Optimization: Least-squares iterations compute local refinements in R^n and accumulate them in the manifold using x_i+1 := x_i ⊞ δ_i.This preserves the classical local refinement procedure while keeping updated states in the manifold.
  • Kalman Filtering: The UKF propagates sigma points through nonlinear process and measurement models without requiring Jacobians, then recovers transformed means and covariances.Its prediction step takes the prior mean, covariance, and control, propagates sigma points, and adds process noise.
  • Kalman Filtering: The manifold UKF represents both states and measurements on manifolds and modifies mean and covariance updates to maintain a consistent reference coordinate system.A simple ⊞ update of the mean can leave the covariance expressed around the old mean, requiring an additional sigma-point propagation.

5. ⊞-Manifolds as a Software Engineering Tool

The Manifold Toolkit encapsulates compound manifold structure so generic algorithms use flattened vectors while application code accesses named, representation-specific components.

  • Toolkit architecture: MTK represents each manifold as a C++ class exposing common boxplus and boxminus operations to generic sensor-fusion algorithms.Primitive implementations include Rn, SO(2), SO(3), and S2.
  • Compound representations: Compound manifolds combine component operators while preserving named member access for users and a unified compound view for algorithms.This corresponds mathematically to Cartesian products of manifold components.
  • Automatic generation: MTK generates compound-manifold classes from component lists, avoiding error-prone manual bookkeeping when many components are present.The generated class includes named members and compound boxplus/boxminus operations.
  • INS example: An INS state can combine three-dimensional position, SO(3) orientation, and velocity into a nine-degree-of-freedom compound manifold.The generated state exposes pos, orient, and vel members.
  • Covariance and optimization: MTK also generates indices for flattened component blocks, enabling covariance initialization and subblock analysis without manually tracking component positions.SLoM additionally infers measurement sparsity and exploits it in Jacobian and matrix computations.

6. Experiments

Experiments apply MTK and UKFoM to synthetic INS-GPS filtering, colored-noise modeling, and representation comparisons, showing concise implementation and robust manifold-based estimation.

  • 6.1. INS-GPS filter: MTK and UKFoM implement a working minimal INS-GPS filter in about 50 lines of C++ code, combining inertial prediction with GPS updates.The framework also supports extending the model from white to colored noise.
  • 6.1.1. IMU Process Model: The process model integrates accelerometer and gyroscope inputs through Euler integration, while named manifold components provide orientation-specific operations.The state update applies an SO(3) exponential to angular velocity and rotates acceleration before updating velocity.
  • 6.1.3. Executing the Filter: The synthetic filter uses 100 Hz IMU data and 4 Hz GPS measurements, with GPS noise standard deviation σp = 0.75 m.The experiment evaluates a predefined trajectory with added white Gaussian sensor noise.
  • 6.1.4. Evaluation on a Synthetic Dataset: GPS measurements reset accumulated inertial drift, and state-component errors largely remain within their 95% probability regions across 50 Monte Carlo runs.The consistency evaluation uses NEES and NMEE measures.
  • 6.1.4. Evaluation on a Synthetic Dataset: Euler-angle and scaled-axis UKFs fail near orientation singularities, whereas the ⊞-method is slightly better than plain quaternions and is described as conceptually more elegant.Reported predict/update times were 21/32 µs for the ⊞ method and 21/33 µs for quaternions.
  • 6.2. Extension to Colored Noise Errors: Adding a GPS-bias state models colored noise and lets MTK and UKFoM rapidly test alternative representations and models without bookkeeping obstacles.The colored-noise simulation uses σ2_b = 5m and T = 1800s.

7. Conclusions

The paper presents ⊞/⊟ as an axiomatized interface that gives generic estimation algorithms a local vector-space view of manifolds while preserving structured state representations.

  • 7. Conclusions: The ⊞ operator applies a small vector-valued perturbation to a manifold state, while ⊟ computes the perturbation between two states.Together they define a ⊞-manifold with a local vector-space view.
  • 7. Conclusions: Replacing vector addition and subtraction with ⊞ and ⊟ adapts least-squares and UKF algorithms to manifolds; UKF mean and covariance updates also change.The framework extends Gaussian, mean, and covariance concepts to ⊞-manifolds.
  • 7. Conclusions: The experiments include 2D DLR data, Stanford parking-garage optimization, and the sphere400 pose dataset, with representation behavior compared through residual-sum-of-squares plots.The supplied passages describe these evaluation settings but do not state a complete aggregate outcome.
  • 7. Conclusions: MTK automatically derives operators for compound manifolds and mediates between flattened algorithmic vectors and structured component views.This makes the mathematical framework a software-engineering toolkit as well as an abstract formalism.

A. Mathematical Analysis of ⊞-Manifolds

The mathematical analysis formalizes ⊞-manifolds as locally Euclidean spaces and establishes their manifold properties through charts, smoothness, and path-connectedness arguments.

  • A. Mathematical Analysis of ⊞-Manifolds: The paper generalizes expected value, covariance, and Gaussian distributions from vector spaces to ⊞-manifolds before proving the framework’s mathematical properties.These concepts support the modified estimation algorithms developed earlier.
  • Manifold foundations: A manifold is modeled as a connected subset of Euclidean space equipped with an atlas of charts mapping open neighborhoods to open subsets of Rn.The dimension n is the number of degrees of freedom.
  • Manifold foundations: Every ⊞-manifold is a manifold, with charts constructed from the local boxplus map and its inverse around each state.The paper explicitly notes that the converse does not hold in general.
  • Proof structure: Connectedness follows by constructing a path γ(λ) = x ⊞(λ(y ⊟x)) between any two states x and y.The endpoints are obtained from the ⊞/⊟ axioms.
  • Chart properties: Smoothness of the local maps makes each chart a diffeomorphism and therefore a homeomorphism onto its image.This supplies the local coordinate structure required by the manifold definition.

A.2. Induced Metric

The ⊞/⊟ framework supports metric, smoothness, symmetry, and product constructions for manifolds while preserving a local vector-space view. It also identifies unresolved case-specific conditions for some axioms.

  • A.2. Induced Metric: The induced distance d(x,y)=∥y ⊟ x∥ satisfies positive definiteness, symmetry, and the triangle inequality under the manifold axioms.The triangle inequality follows by comparing perturbation vectors through a common reference state and applying the ordinary vector-space inequality.
  • A.2. Induced Metric: Smoothness of a map between ⊞-manifolds is equivalent to smoothness of its local perturbation representation.The local expression f(x ⊞S δ) ⊟M z must be smooth in δ at δ=0 for suitable z.
  • A.2. Induced Metric: Symmetry enables defining ⊞ at one reference element and transporting it to other states through diffeomorphic transformations.The construction uses a locally diffeomorphic surjection ψ from R^n to the manifold, a global right inverse, and transformations Rx mapping the reference element to x.
  • A.2. Induced Metric: For connected Lie groups, exponential and logarithm maps provide the local coordinates, yielding x ⊞ δ=x·exp(δ) and y ⊟ x=log(x^-1·y).The remaining axiom is not established generically and instead requires case-by-case proof.
  • A.2. Induced Metric: ⨉-products of ⊞-manifolds inherit smoothness and the first three axioms componentwise, with the remaining condition checked through the component structures.The Cartesian product construction uses V=V1 × V2.

A.9. (Gaussian) Distributions on ⊞-Manifolds

Distributions on ⊞-manifolds must account for the local non-injectivity of state displacement. Restricting perturbations to a valid neighborhood restores a tractable relationship between likelihood and least-squares estimation.

  • A.9. (Gaussian) Distributions on ⊞-Manifolds: Because ⊞ is generally non-injective, multiple Euclidean perturbation distributions can map to the same manifold distribution, complicating probability evaluation and likelihood maximization.The ambiguity arises when defining Y=μ ⊞ X for a Euclidean random variable X.
  • A.9. (Gaussian) Distributions on ⊞-Manifolds: The pragmatic remedy is truncating X to the neighborhood V where ⊞ is unambiguous, defining p(X̃=x)=p(X=x | X∈V).This makes displacement bijective for perturbations restricted to V.
  • A.9. (Gaussian) Distributions on ⊞-Manifolds: When V is large relative to the covariance, the probability outside V is small and the cutoff error is negligible.The paper notes that non-Gaussian noise can produce a larger practical error than this cutoff.
  • A.9. (Gaussian) Distributions on ⊞-Manifolds: For normally distributed perturbation noise, maximum likelihood estimation is equivalent to least-squares minimization.The equivalence follows by expressing the measurement likelihood through the mapped difference f(x) ⊟ z and taking the negative log-likelihood.

B.1. The Rotation Group SO(n)

The rotation group SO(n) is handled as a ⊞-manifold through Lie-group structure, using exponential and logarithm maps for local perturbations and state differences. The construction satisfies the core axioms, with some general cases remaining conjectural.

  • B.1. The Rotation Group SO(n): SO(n) is a Lie group, so its ⊞ and ⊟ operators can be constructed from group multiplication, exponential maps, and logarithms.The matrix exponential maps antisymmetric-matrix perturbations to rotations, while the logarithm provides the inverse representation.
  • B.1. The Rotation Group SO(n): For SO(2) and SO(3), analytic formulas are available, while general numerical algorithms are cited for the matrix exponential and logarithm.The remaining axiom is proved for n=2 and n=3 through isomorphisms, but is conjectured more generally with V=Bπ(0).
  • B.1. The Rotation Group SO(n): The construction x ⊞ δ=x·exp(δ), y ⊟ x=log(x^-1·y) satisfies axioms (11a)–(11c) for a suitable local neighborhood V.For compact Lie groups, the exponential map is surjective and has a global inverse log.
  • B.1. The Rotation Group SO(n): For unit spheres, a Householder reflection maps the reference vector e1 to x, extending the symmetry-based construction beyond Lie groups.The unit sphere is not generally a Lie group, but its symmetries still support a ⊞-manifold structure.
  • B.1. The Rotation Group SO(n): For S^n, exp and log are smooth and satisfy the first three axioms on V=Bπ(0), while the induced metric corresponds to the angle between states.The paper establishes the remaining axiom separately in Lemma 11.
  • B.1. The Rotation Group SO(n): Stereographic projection cannot define a ⊞-manifold because it violates axiom (11b).This illustrates that not every useful manifold parameterization satisfies the required operator properties.

B.3. 2D Orientation as an Orthonormal Matrix

Planar orientations admit equivalent representations through rotation matrices, periodic angles, and unit complex numbers. The exponential and logarithm formulas connect local perturbations with matrix-based state differences.

  • B.3. 2D Orientation as an Orthonormal Matrix: For planar rotations, exp maps a scalar angle to the corresponding 2×2 rotation matrix, while log recovers the angle using atan2(x21, x11).The exponential uses the standard sine-and-cosine rotation matrix.
  • B.3. 2D Orientation as an Orthonormal Matrix: The ⊞/⊟ formulas for planar rotations implement multiplication by a local exponential and recover differences through the logarithm.These operations provide the state update and mapped state difference used by the manifold representation.
  • B.3. 2D Orientation as an Orthonormal Matrix: The exponential map is an isomorphism between R/2πZ and SO(2), because its periodicity and addition-to-multiplication rule agree with the group structure.Consequently, R/2πZ and SO(2) are isomorphic ⊞-manifolds and axiom (11d) holds for SO(2).
  • B.3. 2D Orientation as an Orthonormal Matrix: Unit complex numbers provide another representation isomorphic to SO(2), and the unit-circle construction is likewise isomorphic to both.The equivalence follows from complex multiplication and the uniqueness of the rotation mapping e1 to a unit-circle point.
  • B.3. 2D Orientation as an Orthonormal Matrix: The general sphere construction cannot be continuous in x for S2, because the hairy ball theorem rules out the required continuous vector field.Thus the discontinuity in the corresponding construction is unavoidable in general, although continuity can hold for S1 and S3.

B.6. 3D Orientation as a Unit Quaternion

The quaternion manifold models rotations through the same local displacement structure used for the unit sphere, with perturbations restricted to a ball of radius π. Its induced metric matches angular distance, while projective spaces require handling antipodal equivalence.

  • Quaternion representation: The quaternion ⊞-manifold is a special case of the unit-sphere construction, using quaternion multiplication and V = Bπ(0).Its axioms (11a)–(11c) hold directly, and Axiom (11d) is established in Lemma 12.
  • Quaternion representation: The induced metric d(x, y) equals the angle between orientations.This gives the ⊟-based distance a direct geometric interpretation for orientation estimation.
  • Quaternion representation: Orthonormal matrices SO(3) and unit quaternions S3/{±1} are distinct rotation representations whose ⊞-manifolds are isomorphic.The conversion preserves the displacement operation, and the isomorphism establishes Axiom (11d) for SO(3).
  • Projective spaces: Projective space Pn identifies nonzero vectors that differ by a nonzero scalar, yielding Pn ∼= Sn/{±1}.For P3, this gives P3 ∼= SO(3); P2 requires addressing the ambiguity x ≡ −x.
  • Projective spaces: The induced metric on projective space is not currently characterized.This remains an explicit limitation of the projective-space construction.

C. Technical Proofs

The technical proofs establish geometric properties of the manifold operators, including geodesic displacement, angular distance, metric inequalities, and isomorphisms between representations. They also relate the framework to SPMap while identifying a continuity trade-off and a broader axiomatized scope.

  • Metric comparison: For unit quaternions, the induced distance is monotonically related to Euclidean distance, including when antipodes are identified.The corresponding SO(3) result establishes an analogous monotonic relation with Frobenius distance.
  • Sphere geometry: The curve x ⊞(λδ) is a geodesic on Sn with arc length ∥δ∥.This follows because the curve is a unit-radius circle segment.
  • Axiom proofs: Axiom (11d) holds for the hypersphere and quaternion ⊞/⊟ operators.The hypersphere result is stated in Lemma 11, while the quaternion result is established in Lemma 12.
  • Sphere geometry: For spherical perturbations, the distance between x ⊞δ1 and x ⊞δ2 follows the spherical law of cosines.The result depends on α = ∥δ1∥, β = ∥δ2∥, and γ = ∠(δ1, δ2).
  • Metric comparison: The sphere distance is less than or equal to the Euclidean distance in the tangential plane.The proof uses convexity after reducing the comparison to λ = cos γ.
  • Relation to SPMap: The framework is more general than SPMap but cannot maintain continuity in both pose and perturbation because of the hairy ball theorem.The authors describe their contribution as an axiomatized view not limited to quotients of SE(3).
Loading 1107.1119v1…