Holistic Fusion: Task- and Setup-Agnostic Robot Localization and State Estimation with Factor Graphs

TL;DR

This paper addresses the challenge of robust, low-latency local state estimation and global localization in diverse, sensor-rich robotics scenarios. It introduces Holistic Fusion (HF), a general factor-graph framework that jointly estimates the robot state, a dynamic set of reference-frame alignments, global calibrations, and landmarks, while enabling direct fusion of measurements across arbitrary frames at IMU rates. Key innovations include automatic reference-frame alignment via drift modeling with random walks, local keyframe alignment to manage long trajectories, and an asynchronous online–offline MAP estimation workflow with a complete set of holistic and standard factors. The framework is validated across five real-world tasks on three robotic platforms, showing improved global localization during GNSS dropout, smoother local odometry, and robust performance in mixed environments, with an accompanying open-source implementation for broad adoption.

Abstract

Seamless operation of mobile robots in challenging environments requires low-latency local motion estimation (e.g., dynamic maneuvers) and accurate global localization (e.g., wayfinding). While most existing sensor-fusion approaches are designed for specific scenarios, this work introduces a flexible open-source solution for task- and setup-agnostic multimodal sensor fusion that is distinguished by its generality and usability. Holistic Fusion formulates sensor fusion as a combined estimation problem of i) the local and global robot state and ii) a (theoretically unlimited) number of dynamic context variables, including automatic alignment of reference frames; this formulation fits countless real-world applications without any conceptual modifications. The proposed factor-graph solution enables the direct fusion of an arbitrary number of absolute, local, and landmark measurements expressed with respect to different reference frames by explicitly including them as states in the optimization and modeling their evolution as random walks. Moreover, local smoothness and consistency receive particular attention to prevent jumps in the robot state belief. HF enables low-latency and smooth online state estimation on typical robot hardware while simultaneously providing low-drift global localization at the IMU measurement rate. The efficacy of this released framework is demonstrated in five real-world scenarios on three robotic platforms, each with distinct task requirements.

Figures (25)

• Figure 1: Overview of a fully autonomous hiking experiment using the ANYmal quadrupedal robot in a forest environment. Online and offline motion estimation are conducted by fusing i) IMU, ii) noisy and degraded GNSS measurements, iii) the online-aligned (drifting) absolute pose of a LO (LO) system expressed in the map frame, and iv) leg odometry. A: Overview of the different trajectories with highlighted drift and GNSS loss. B: The aligned LO trajectory at three keyframes. C: 3D and 2D visualizations of the random walk are used to model the evolution of the map frame location.
• Figure 2: High-level overview of HF, its role and its functionalities. Not only is HF acting as a central fusion module, it also i) estimates the relative context between the reference frames of the measurements or modules, and it ii) allows for either high- or low-rate beliefs to be fed back to the modules.
• Figure 3: An illustrative scenario of HF. During the mission, different measurements are fused directly without preprocessing (in the shown example: IMU, GNSS, LO, local velocity). To fuse global and non-global absolute measurements, which drift against each other (blue path), HF explicitly estimates the shift between the reference coordinates frames, aligns the measurements, and models them as a random walk. This alignment is not conducted at the global origin but at local keyframes. The graph at the top shows a simplified version of the resulting graph with factors and states depicted in the same colors.
• Figure 4: Structural overview of the factor graph design of HF proposed in this work. As depicted, IMU measurements drive the creation of the robot state. All other states are created dynamically (setup-specific) based on the provided measurements and framework configuration: a) Reference frame alignment states. b) Landmark states. c) Global states. d) Example measurement types depicted as factors in the graph. Right side: Steps from \ref{['alg:method_graph_creation']}.
• Figure 5: Illustrated benefit of local keyframe alignment (bottom) over global origin alignment (top), which can lead to instability with increased distance.