Zi Cong Guo

Marginalizing and Conditioning Gaussians onto Linear Approximations of Smooth Manifolds with Applications in Robotics

Zi Cong Guo, James R. Forbes, Timothy D. Barfoot

International Conference on Robotics and Automation (ICRA), 2025

★ ICRA Best Conference Paper Award

We extend Gaussian marginalization and conditioning from axis-aligned linear manifolds to general linear manifolds, and use this result to derive a principled local approximation for smooth nonlinear manifolds. This provides theoretically justified inference operations for Gaussian distributions constrained to manifold structure. In robotics, the framework applies naturally to constrained estimation problems, enabling covariance extraction by conditioning an unconstrained Gaussian approximation onto linearized constraints.

Many robotics estimation problems involve states that must satisfy geometric or physical constraints. These constraints may come from the state representation, such as rotations and poses, or from the estimation problem itself. After estimating the optimal states, practitioners often need covariance estimates that remain meaningful on the underlying feasible space. This paper provides principled marginalization and conditioning tools for Gaussian approximations on such spaces, enabling uncertainty analysis in constrained optimization problems. We demonstrate the approach on a few applications, including constrained localization implemented in GTSAM.

Robots need to estimate many things about themselves and the world around them, including their position, orientation, motion, and the surrounding map. For planning and safety purposes, they also need to understand how uncertain these estimates are. Standard mathematical tools can represent this uncertainty, but the problem becomes harder when the estimates must obey constraints, such as keeping rotations valid or enforcing contact between a robot arm and a box. This work extends those tools to constrained settings, so the resulting uncertainty estimates remain meaningful for many robotics applications where such constraints naturally arise.

paper / arxiv / bibtex / video

KILO-EKF: Koopman-Inspired Learned Observations Extended Kalman Filter

Zi Cong Guo, James R. Forbes, Timothy D. Barfoot

Accepted to International Conference on Intelligent Robots and Systems (IROS), 2026

This paper introduces a Koopman-inspired learned observation model for extended Kalman filtering (EKF) on Lie-group states. Instead of relying on a handcrafted nonlinear measurement model, the method learns a lifted measurement model that is approximately linear in the state, enabling standard EKF measurement updates while preserving the filter's prediction and uncertainty propagation structure. The approach is validated on real-world localization datasets with ultra-wideband range measurements.

Many robot localization systems rely on analytical sensor models that can be difficult to derive accurately, especially when the sensing process is noisy, biased, or affected by the environment. This work introduces KILO-EKF, which keeps the familiar structure of an extended Kalman filter but replaces the measurement model with a learned model trained from data. Since our learned model is linear in a lifted feature space, KILO-EKF allows practitioners to improve filtering accuracy while retaining the interpretability and modularity of a standard filtering pipeline.

Robots often estimate their poses by comparing sensor readings to what a mathematical model predicts. When that model is inaccurate, the robot's estimates can suffer. This work lets the robot learn parts of the sensor model from data, so the model can better reflect the sensor and environment in which the robot operates. This improves pose estimation while keeping the system grounded in well-established localization theory, with understandable computations and meaningful uncertainty estimates.

arxiv / bibtex

Koopman Linearization for Data-Driven Batch State Estimation of Control-Affine Systems

Zi Cong Guo, Vassili Korotkine, James R. Forbes, Timothy D. Barfoot

Robotics and Automation Letters (RA-L), 2022

This paper presents the Koopman State Estimator (KoopSE), a Koopman-inspired batch state estimation framework for nonlinear control-affine systems. By learning lifted representations of both the process and measurement models, the method approximates the nonlinear estimation problem with a linear-Gaussian structure in feature space. This enables standard batch estimation techniques to be applied to systems whose dynamics or sensors are difficult to model analytically.

Many robotics systems rely on process and measurement models that are difficult to derive accurately, especially when the dynamics are complex or only partially known. This work introduces KoopSE, which learns these models from data and embeds them into a batch estimation framework, allowing the full trajectory to be estimated using familiar optimization-based tools. This provides a way to combine data-driven modeling with structured state estimation, in contrast to purely black-box predictors that replace the estimator entirely.

Robots estimate their motion by combining what they expect to happen with what their sensors receive. When either the motion model or the sensor model is inaccurate, the resulting estimates can suffer. This work lets the robot learn both relationships from data, so the models can better reflect the system and environment in which the robot operates. These learned models are then used inside a structured estimation method, improving estimation accuracy while keeping the overall approach interpretable and grounded in well-established robotics theory.

paper / arxiv / bibtex

Data-Driven Batch Localization and SLAM Using Koopman Linearization

Zi Cong Guo, Frederike Dümbgen, James R. Forbes, Timothy D. Barfoot

Transactions on Robotics (T-RO), 2024

This paper extends Koopman-inspired batch estimation to localization and SLAM by introducing the Reduced Constrained Koopman Linearization (RCKL) framework. The method jointly estimates robot states and landmarks using lifted models learned from data, revealing new challenges that arise in larger-scale lifted estimation problems. In particular, unconstrained optimization in the lifted space can drift away from representations that correspond to valid states and landmarks. RCKL addresses this by enforcing consistency with the constraint manifolds of the original robotics problem, enabling data-driven models to be used in SLAM while preserving the required manifold structure.

We propose the RCKL framework, which uses lifted models learned from data to improve localization and mapping when analytical process or measurement models are difficult to specify. To make lifted estimation practical for SLAM, our method introduces consistency constraints that keep the estimated robot trajectory and landmarks tied to the original physical problem. The resulting constrained optimization problem can be solved efficiently with sequential quadratic programming while exploiting sparsity structure similar to that used in classical batch SLAM solved with Gauss Newton.

Robots often need to estimate both where they are and what the surrounding map looks like. When the models used for this estimation are inaccurate, the robot's trajectory and map can become less reliable. This work lets the robot use models learned from data, so the estimation can better reflect the system and environment in which the robot operates. At the same time, it adds constraints that keep the estimated trajectory and map physically meaningful as valid real-world configurations. The method remains efficient by building on well-established localization and mapping techniques, combining data-driven modeling with a structured and interpretable estimation framework.

paper / arxiv / bibtex

Smoothing Out the Edges: Continuous-Time Estimation with Gaussian Process Motion Priors on Factor Graphs

Connor Holmes, Sven Lilge, Zi Cong Guo, Frank Dellaert, Timothy D. Barfoot

Accepted to Foundations and Trends in Robotics, 2026

Gaussian-process (GP) continuous-time state estimation provides a nonparametric way to represent smooth trajectories, useful for handling asynchronous measurements and interpolating between states, but it has seen less adoption than spline-based approaches in robotics. This paper reframes GP continuous-time estimation in the language of factor graphs, showing that trajectory interpolation and querying can be understood through standard elimination operations. The result connects GP trajectory estimation to the factor-graph framework widely used in localization and SLAM, with practical implementations and examples provided in GTSAM.

Many localization and SLAM systems must combine asynchronous measurements while estimating the robot trajectory between sensor times. Common examples include rolling-shutter cameras and sensors operating at different rates. Modeling the trajectory as a continuous-time Gaussian process helps resolve these timing mismatches by representing motion as a smooth path that can be queried at any time. The paper shows how to implement this approach within standard factor-graph pipelines in the widely used GTSAM library, with practical interfaces and examples for adoption in real-world robotics systems.

Robots often receive information from cameras, lasers, and other sensors operating at different times. In these situations, we can improve navigation by estimating the robot's motion as a smooth path rather than a sequence of disconnected snapshots. This work demonstrates that these methods can be understood and implemented using the same efficient estimation tools already used in many robotics systems.

arxiv / blog / bibtex