In the past couple years, a new type of foundation models called the Geometric Foundation Model (GFM) has been creating a lot of excitement for 3D scene reconstruction starting with initial works DUSt3R and MASt3R. GFMs take in uncalibrated monocular RGB images and output a dense 3D point cloud and camera poses. One of the most popular models, VGGT, won best paper at CVPR 2025 and follow-up work VGGT-Omega was a best paper finalist at CVPR 2026. Their simplicity and ability to create dense reconstruction without depending on known camera calibration or stereo rigs begs the question of how to best leverage them for a robotic SLAM system. In this post, we’ll discuss how VGGT-SLAM (and its extension VGGT-SLAM 2.0) does just that.

Using VGGT for a SLAM System

The first challenge in bringing GFMs to SLAM is robots may need to process many thousands of images; however, GPU memory bounds how many frames VGGT can process (around 60 on a 3090 GPU with 24 GB of memory). The second challenge is most practical use cases require incremental mapping as the robot explores a scene – not just one large batch processing of images for the entire scene.

Both challenges can be solved by a simple idea: create smaller submaps with VGGT as the robot explores a scene and chain these submaps together to create a global map. Sounds easy enough; now we just need to pick which transformation to use to align the submaps. Classical SLAM logic tells us $\text{Sim(3)}$ should do it. Each VGGT submap is defined in its own local frame ($\text{SE(3)}$ alignment is needed) and since VGGT doesn’t estimate metric scale we need the extra DoF of $\text{Sim(3)}$ to align the submaps. Unfortunately, this $\text{Sim(3)}$ alignment isn’t always enough. As an example, chaining two submaps together in this apartment scene with a $\text{Sim(3)}$ transformation shows poor alignment.

The need for a higher DoF transformation

The missing piece is VGGT does not know the camera calibration. While it tries to estimate the calibration, this uncertainty causes the submaps to have a higher DoF ambiguity. To understand what transformation we need for this state-of-the-art foundation model, we find the answer buried deep in what’s considered the bible of classical computer vision – Multiple View Geometry in Computer Vision by Hartley and Zisserman. Which is kind of amusing since during their CVPR talk, the VGGT authors had a slide saying “You don’t have to be Zisserman” to do 3D reconstruction anymore. Anyway, chapter 10 (second edition) mentions “The Projective Reconstruction Theorem” which in summary states that if you have a set of images and solve for a 3D reconstruction given known pixel correspondences, the reconstruction has a 15 DoF projective ambiguity to the true scene. Given extra information such as the vanishing point, this can be reduced to a 12 DoF affine ambiguity.

This 15 DoF transformation is a $4 \times 4$ homography matrix (the lesser known 3D version of the common $3 \times 3$ homography used in 2D vision tasks like image stitching). Now that we know to use a 15 DoF projective transformation, we can solve for the homography between submaps. The homography can be estimated with a 5-point RANSAC solver.

Looking at the apartment example from before, we get a much cleaner submap alignment using the homography matrix.

Integration with GTSAM

Now for the really cool part. We can normalize the $4 \times 4$ homography matrix to have determinant 1 which maps it to a unique matrix on the Special Linear Group, $\text{SL(4)}$, manifold. $\text{SL(4)}$ is the group of $4 \times 4$ matrices with determinant 1. This lets us chain submaps together (along with loop closure constraints) and use GTSAM to create a factor graph optimized on the $\text{SL(4)}$ manifold. Support for $\text{SL(4)}$ factors was added to GTSAM in PR #2207 and has a similar interface as $\text{SE(3)}$ factors.

VGGT-SLAM 2.0

One downside to solving 15 DoF transformations for submap alignment is high-dimensional drift can quickly build up without loop closures. Additionally, a 5-point solver for the homography matrix requires the 5 points not all be co-planar - which can cause degeneracy when submaps view flat floors or a single wall. To get around this, VGGT-SLAM 2.0 maintains an $\text{SL(4)}$ factor graph but recognizes the VGGT submap alignment problem can be constrained to a subset of variables. For example, consecutive submaps are created so that they share a common keyframe - the first keyframe of submap $n$ is the same keyframe as the last one of submap $n-1$. This means that their respective translation and rotation must be trivially identical in the world frame. Likewise, while the true calibration is unknown, we know that the shared frames must come from a camera with the same calibration, allowing us to further reduce the variables when estimating projective alignments.

Multiple additional improvements, such as more reliable loop closures, are also included in the VGGT-SLAM 2.0 paper and a demonstration that the entire SLAM system can run in real-time onboard a robot with a Jetson Thor.

Further browsing

• Example of using $\text{SL(4)}$ factors are available in a GTSAM example notebook
• ArXiv: VGGT-SLAM
• ArXiv: VGGT-SLAM 2.0
• ArXiv: FOUND-IT

Global Navigation Satellite System (GNSS) positioning is foundational to autonomous driving, surveying, and precision agriculture. Standard single-point positioning achieves meter-level accuracy, but many applications need tighter estimates. Real-Time Kinematic (RTK) GNSS moves toward centimeter-level positioning by exploiting double-difference observations that cancel or reduce common-mode errors such as satellite clock biases and atmospheric delays.

GTSAM now includes built-in double-difference factors for both pseudorange and carrier-phase observations, contributed in PR #2502. This post explains the technique, walks through the new factors, and shows how they enable tightly-coupled GNSS-IMU fusion via factor graphs.

Why Double Differencing?

A raw GNSS pseudorange measurement from a receiver $r$ to a satellite $s$ can be written as:

where $\rho_r^s$ is the geometric range, $c$ is the speed of light, $\delta t_r$ and $\delta t^s$ are the receiver and satellite clock biases, $I$ and $T$ represent ionospheric and tropospheric delays, and $\epsilon_P$ is the measurement noise.

Single differencing between a rover receiver and a nearby base station eliminates the satellite clock bias $\delta t^s$ and greatly reduces atmospheric terms that are spatially correlated. Double differencing then takes the difference between two satellites (a reference satellite and a target satellite), which additionally eliminates the receiver clock bias $\delta t_r$. Using the operator $\Delta\nabla$ to denote the double difference:

where $i$ is the reference satellite, $j$ is the target satellite, $r$ is the rover, $b$ is the base station, and $\epsilon_{\Delta\nabla P}$ is the double-differenced noise. The result is a measurement that depends almost entirely on the rover position and geometry. A reference satellite is chosen per constellation (GPS, Galileo, BeiDou, QZSS, …), typically the one with a high elevation angle and strong signal strength.

For carrier-phase observations, the double-difference of the phase measurement $\Phi$ is:

where $\Phi$ is the carrier-phase observable in meters, $\lambda$ is the carrier wavelength, $N$ is the integer ambiguity, and $\epsilon_{\Delta\nabla \Phi}$ is the noise. Carrier-phase measurements are far more precise than pseudorange (millimeter-level noise vs. meter-level), so resolving these integer ambiguities is the key to centimeter-level positioning.

What GTSAM Now Provides

GTSAM’s navigation module now includes four double-difference factors along with shared helpers, all contributed in PR #2502.

For each satellite pair, the user adds a pseudorange factor and a carrier-phase factor to the graph. The pseudorange factor is a unary factor on the rover position, while the carrier-phase factor additionally connects to ambiguity variables $N$ that persist across epochs (as long as no cycle slip occurs).

The basic factors, DoubleDifferencePseudorangeFactor and DoubleDifferenceCarrierPhaseFactor, take a Point3 antenna position in ECEF directly. The lever-arm variants, DoubleDifferencePseudorangeFactorArm and DoubleDifferenceCarrierPhaseFactorArm, take a Pose3 in the navigation frame plus a body-frame lever arm, computing the antenna position internally. These are essential for tightly-coupled IMU fusion, where the optimized state is the vehicle pose.

A shared helper, gnss::DoubleDifferenceData, bundles the rover/base observations and satellite positions for a given satellite pair and provides the Sagnac-corrected geometric range model $\Delta\nabla\rho(\cdot)$ with Jacobians. This keeps the individual factors thin. The pseudorange factor is unary in the rover antenna position $\mathbf{x}$ and simply evaluates the model-minus-observation residual:

The carrier-phase factor is ternary, connecting the rover position $\mathbf{x}$ to the two satellite ambiguities $N_\text{ref}$ and $N_\text{target}$, and adds the $\lambda \cdot (N_\text{ref} - N_\text{target})$ term on top:

where $\Delta\nabla\tilde{P}$ and $\Delta\nabla\tilde{\Phi}$ are the double-differenced pseudorange and carrier-phase observations. For the lever-arm variants the state $\mathbf{x}$ is a Pose3 and the antenna position is obtained from the pose and body-frame lever arm before the same residual is evaluated.

GTSAM treats each $N$ as a continuous variable during optimization (the “float” solution). Integer fixing is done outside the graph: at each epoch the float estimates and covariance are handed to the LAMBDA algorithm, which searches for the most likely integer vector. Validated fixes are then enforced in one of two ways (fix-and-hold): either a tight prior factor is added on the corresponding $N$ variables, or the integer values are substituted as constants inside the carrier-phase factors so that $N$ no longer appears as a variable at all. A fixed ambiguity is held until a cycle slip on that satellite invalidates it, at which point the corresponding $N$ is reset and re-estimated as a float.

Tightly-Coupled GNSS-IMU Factor Graph

The diagram below illustrates how these factors fit into a factor graph for tightly-coupled GNSS-IMU positioning. At each epoch, the rover pose is connected to a pair of double-difference factors per reference/target satellite pair: a pseudorange factor (red), which is unary on the pose, and a carrier-phase factor (blue), which additionally connects to both ambiguity variables of the pair — the reference-satellite ambiguity $N_\text{ref}$ and the target-satellite ambiguity $N_\text{target}$ — which persist across epochs. IMU pre-integration factors connect consecutive poses, bridging the gap when satellite signals are blocked.

By using the lever-arm factor variants, the rover state becomes a Pose3 in the navigation frame, which can be directly connected to GTSAM’s IMU pre-integration factors. This tightly-coupled approach uses raw satellite measurements directly. The full covariance structure between position, velocity, and biases is maintained throughout the graph, and when buildings block satellite signals in urban canyons, the IMU bridges the gap while GNSS constrains long-term drift.

In practice, achieving centimeter-level accuracy requires additional infrastructure beyond the GTSAM factors themselves: satellite selection, cycle-slip detection, multipath mitigation, and integer ambiguity resolution (typically via the LAMBDA algorithm). Our implementation uses cssrlib-numba for these observation-modeling tasks.

Results on PPC-Dataset

We evaluated the tightly-coupled system on the Tokyo sequences of the PPC-Dataset, an open dataset of urban driving in Japan. Tokyo’s dense urban canyons present a particularly challenging environment for GNSS positioning: tall buildings block direct line-of-sight to many satellites, and reflections produce multipath that corrupts both pseudorange and carrier-phase observations.

The evaluation uses the lever-arm DD factors together with CombinedImuFactor, non-holonomic constraint factors, and integer ambiguity variables resolved via LAMBDA with fix-and-hold. The graph is solved incrementally with IncrementalFixedLagSmoother.

The three runs achieve 49.5–60.8% fix rates and 56.7–69.9% of epochs within 50 cm error. Fixed epochs are mostly accurate to a few centimeters, but occasional incorrect fixes inflate the RMS to 0.21–0.81 m.

With GNSS and IMU alone, the estimate drifts during long GNSS outages (under overpasses or in tunnels). Adding LiDAR or wheel odometry factors on the same poses keeps the estimate stable through these gaps.

Takeaway

GTSAM now provides native support for RTK GNSS double-difference pseudorange and carrier-phase factors, an important step toward centimeter-level positioning in full GNSS pipelines. These factors integrate naturally with the existing factor graph framework, enabling tightly-coupled multi-sensor fusion with IMU, wheel odometry, or any other GTSAM factor. The lever-arm variants make it straightforward to model the physical offset between the navigation state and the antenna position.

Additional Reading

• The DD factors are available in GTSAM’s navigation module (PR #2502)
• gnss-gtsam-rtk: A standalone RTK example using GTSAM’s DD factors with ISAM2
• tightly-coupled-gnss-imu-fgo: Tightly-coupled GNSS-IMU factor graph optimization for urban driving
• PPC-Dataset: Open dataset with multi-frequency GNSS and 100 Hz IMU data from urban environments in Japan

Disclosure: AI was used to help draft this post.

This third and final ICRA-week post zooms out to the broader global-solvers ecosystem. Part 1 focused on our chordal-sparsity work, which uses Bayes-tree structure to make SDP relaxations more scalable. Part 2 focused on David Rosen’s group’s Certifiable Factor Graph Optimization, which approaches related factor-graph problems through Burer-Monteiro-style low-rank optimization. But these are only two efforts in a broader push toward global solvers for 3D vision and robot perception, a lot of it started by Luca Carlone & collaborators over the last 10 years. At ICRA this year we were very fortunate to have a workshop keynote by Heng Yang, his former Ph.D. student and now a professor at Harvard:

• Scaling Semidefinite Relaxations for Robot Perception and Control at the Frontiers of Optimization for Robotics workshop.

So, in this Part 3, I want to draw some attention to Luca and Heng’s seminal work in this area

A short lineage

One way to see the field’s trajectory is through rotation estimation, pose-graph optimization, and registration. Our 2015 ICRA paper, Initialization Techniques for 3D SLAM, was about making hard nonconvex SLAM problems behave better by separating out the rotation-estimation structure before running local pose-graph optimization. At MIT, David Rosen pioneered SE-Sync, which showed how pose synchronization over SE(d) could be solved efficiently while certifying global optimality in the relevant noise regime. I also worked with David and Luca and others on Shonan Rotation Averaging, that connected SDP relaxation and manifold optimization to large-scale rotation averaging.

Heng and Luca then pushed certifiable registration toward extreme outlier robustness in the context of point cloud registration, with TEASER, by Heng Yang, Jingnan Shi, and Luca Carlone. This paper already has > 1100 citations, and Heng has been on a convex relaxation binge since then!

Global solvers for 3D vision

A lot of the recent developments in this area, by Heng and others, were collected in the workshop paper Global Solvers for 3D Vision: Foundations, Frontiers, and a Call to the Robotics Community. A longer-form version with Heng as a co-author is available on Arxiv here: Advances in Global Solvers for 3D Vision.

If you’re interested in knowing more, that survey is a very useful entry point. The authors introduce branch-and-bound (B&B), convex relaxation (CR), and graduated non-convexity (GNC) in the context of geometric estimation problems that show up constantly in robot perception, for example absolute and relative pose estimation, 3D registration, rotation and translation averaging, and even full bundle adjustment.

Back to GTSAM

The recent QP and QCQP support in GTSAM is a preview of the modeling layer needed to lift, relax, decompose, and eventually certify factor-graph problems. We hope to bring certifiable optimization to GTSAM soon, and I hope this will allow more robotics people to join in the fun!

Further browsing

• Workshop paper: Global Solvers for 3D Vision
• ArXiv: Advances in Global Solvers for 3D Vision
• OpenReview: Certifiable Factor Graph Optimization
• ArXiv: SE-Sync
• ArXiv: TEASER
• ArXiv: Shonan Rotation Averaging
• Post: Certifiable Optimization at the ICRA Workshop
• Post: Quadratic Programs and QCQPs in GTSAM

Disclosure: AI was used to help draft this post and prepare the figure.

Certifiable estimation is rapidly maturing as a practical tool for robust robotic perception and state estimation, enabling both fast and certifiably correct (i.e. verifiably globally optimal) inference. Frank’s companion post highlights one way to implement certifiable estimators in GTSAM by exploiting the Bayes Tree to perform chordal decomposition; here we describe complementary work from Northeastern that exploits Burer-Monteiro factorization to implement certifiable estimation using the factor graph modeling and local optimization paradigm already familiar to users of GTSAM.

Our workshop paper is:

Certifiable Factor Graph Optimization, by Zhexin (Jason) Xu, Nikolas R. Sanderson, Hanna Jiamei Zhang, and David M. Rosen. Workshop page: OpenReview.

Two paradigms for robotic state estimation

Factor graphs are well-established as the dominant paradigm for modeling and solving robotic state estimation tasks, primarily because they are so wonderfully easy to use. The factor graph abstraction that GTSAM is built on lets you easily model a wide range of estimation problems by composing a handful of standard variable and factor types; moreover, GTSAM can automatically synthesize and run fast local optimizers to perform inference directly from a factor graph model. The catch is reliability: because factor graph inference is typically performed using local optimization, it can sometimes silently converge to a badly wrong estimate, even on a well-posed problem.

More recently, certifiable estimation has emerged as a powerful new approach for implementing trustworthy robotic perception and state estimation systems. The main idea behind certifiable estimators is to construct a convex (typically semidefinite) approximation of the target maximum likelihood estimation problem, and then solve this convex surrogate to recover a high-quality solution to the original estimation task. This approach has three major advantages. First, because the surrogate is convex, it can be solved globally. Second, its minimizer often turns out to be an exact, globally optimal solution of the original problem. Third, these methods yield an a posteriori certificate of optimality whenever they succeed — that is, they can tell you whether they actually found the right answer.

However, the catch to using certifiable estimation methods is effort: the SDP relaxations underpinning these methods are typically very high-dimensional, and therefore require specialized, structure-exploiting optimization techniques in order to solve them efficiently. The standard pipeline for deploying a high-dimensional certifiable estimator thus involves developing a problem-specific SDP relaxation, designing a custom-built local optimizer for its (low-dimensional) Burer-Monteiro factorization, and then wrapping that optimizer in the Riemannian Staircase to ensure global optimality. Executing this process can require weeks to months of specialized effort, which must be repeated from scratch for every new problem.

The key insight: Certifiable estimation inherits factor graph structure

The standard pipeline treats the SDP machinery and the factor graph model as separate worlds. Our central observation is that they aren’t: the two transformations at the heart of certifiable estimation — Shor’s relaxation and Burer-Monteiro factorization — preserve the factor graph structure of the problem that they start from.

The reason is almost embarrassingly simple. The maximum likelihood problems we care about can be written as quadratically-constrained quadratic programs (QCQPs), and the factor graph structure of such a QCQP is encoded directly in the data matrices that define it: factor connectivity shows up as block sparsity in the objective matrices, and the product structure of the feasible set shows up as block-diagonal constraint matrices. The key point is that the same data matrices that define the original QCQP also define its Shor relaxation, as well as every Burer-Monteiro factorization of that relaxation. None of this structure is lost along the way!

The consequence is that the Burer-Monteiro-factored Shor relaxation automatically inherits a factor graph structure from the original estimation problem. This induced factor graph has identical connectivity to the original; only the variables and factors themselves change. And these change in the simplest possible way: each is a (slightly) higher-dimensional, one-to-one algebraic transformation — a lift — of its counterpart in the original factor graph. For example, a rotation variable lifts to a Stiefel-manifold variable, a unit vector lifts to a higher-dimensional unit vector, a translation lifts to a (higher-dimensional) translation, and the factors lift accordingly. We call the resulting factor graph a lifted (or certifiable) factor graph.

Certifiable estimation is factor graph optimization

Because the Burer-Monteiro-factored Shor relaxation is itself a factor graph, the local optimizations appearing inside the Riemannian Staircase can be carried out by an ordinary factor graph optimizer — exactly what GTSAM is built to do. Consequently, the Riemannian Staircase meta-algorithm for certifiable estimation collapses to a thin wrapper around standard local factor graph optimization:

Input: factor graph G for a QCQP-representable MLE problem
for p = d, d+1, ... do
    build lifted factor graph G_p for the rank-p BM factorization of G
    Y* <- LocalOptimization(G_p)        # ordinary factor graph optimization
    if Z = Y*(Y*)^T certifiably solves Shor's relaxation:
        return Y*                       # globally optimal!
end

For anyone already comfortable with factor graphs, this turns certifiable estimation from a research project into a modeling exercise. You take a factor graph model of your problem, replace each variable and factor with its lifted counterpart, and hand the result to the optimizer you already use. No problem-specific SDP derivation, no custom Riemannian solver, no hand-analysis of manifold geometry — and yet the Staircase still guarantees recovery of a globally optimal solution, with a certificate.

Experiments

We implemented our certifiable factor graph optimization framework in GTSAM and evaluated it on three problem classes — pose-graph optimization, landmark SLAM, and range-aided SLAM — across a broad set of standard benchmarks. Two findings stand out:

• It matches purpose-built certifiable estimators. Our general-purpose factor graph-based estimator recovers the same objective values and the same certified suboptimality bounds as the specialized, hand-engineered solvers SE-Sync, Landmark SE-Sync, and CORA — all because it is solving the very same underlying relaxations.

• It preserves the speed of local factor graph optimization. When the initialization is already good, the method terminates at the first level of the Staircase after a single local solve (plus a cheap verification step), behaving just like ordinary factor graph optimization. It only invokes the full Staircase machinery when global optimality cannot be certified, and even then the cost scales roughly linearly in the number of levels.

In short, you get the global-optimality guarantees of a specialized certifiable estimator at essentially the cost of a standard local solve — and the development effort drops from the weeks-to-months of the bespoke pipeline to a few hours of modeling with standard GTSAM factor graphs.

Why this matters for GTSAM

Certifiable factor graph optimization provides a path for bringing certifiable estimation into the software stack roboticists already use. Instead of choosing between a convenient local factor graph model and a separate bespoke certifiable solver, the same model can now support both fast local optimization and global optimality guarantees.

This fits naturally with GTSAM’s recent QP and QCQP support. QCQPs provide the algebraic bridge from factor graph estimation problems to semidefinite relaxations, while GTSAM already provides the factor graph abstraction, sparse optimization machinery, and many of the variable and factor types used in robotics. Our implementation in the paper is built in C++ using GTSAM, and the broader goal is to make certifiable estimation a reusable part of the factor graph workflow rather than a separate custom project for each new problem.

Further browsing

• Frontiers of Optimization for Robotics workshop
• Arxiv: Certifiable Factor Graph Optimization
• OpenReview: Certifiable Factor Graph Optimization
• The SLAM Handbook (Chapter 6)
• Post: Certifiable Optimization at the ICRA Workshop
• Post: Quadratic Programs and QCQPs in GTSAM

Disclosure: AI was used to help draft this post and prepare the figures.

This post focuses on our chordal-sparsity paper, while last week’s post already covered CMC-Opt and our new legged-robot estimators, including Yetong Zhang’s workshop paper on constraint manifolds with corners. Both papers will be presented at the Frontiers of Optimization for Robotics workshop at ICRA shows how much momentum there is right now around certifiable optimization, convex relaxation, and structure-exploiting solvers for robotics.

Chordal sparsity

The chordal-sparsity paper is here:

Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs, by Avinash Subramanian, Connor Holmes, Timothy D. Barfoot, Frank Dellaert, and Frederike Dümbgen.

The paper combines factor-graphs and structured optimization via the Bayes-tree with the certifiable-estimation viewpoint that Connor, Tim, and Frederike have helped push forward at the University of Toronto. Local solvers such as Gauss-Newton and Levenberg-Marquardt are fast and exploit sparsity beautifully, but they do not promise that the answer is globally optimal. Convex SDP relaxations can give global solutions or certificates, but the naive monolithic SDP is usually too expensive.

Our contribution is to make the relaxation respect the graph structure. Starting from a GTSAM factor graph, we lift the problem to a QCQP, construct the Bayes tree through variable elimination, and use the resulting cliques to build a chordally decomposed SDP. In plain terms: instead of solving one enormous positive semidefinite matrix problem, we solve many smaller clique-wise matrix problems that follow the same sparse structure GTSAM already understands.

The graph above shows the benefit of using the cliques of the Bayes tree: the chordal approach is not as fast a a local solver, but it is globally optimal, at a cost that is vastly less than a monolithic SDP solver, which OOMs on problems of moderate size.

The certifiable wave

The workshop has several papers that combine certifiable optimization, convex relaxations, and structure-exploiting solvers, which is fast becoming a theme in both estimation and control. Our chordal-sparsity paper is part of the “certifiable wave”, but here are many other papers touching on the theme:

• Certifiable Factor Graph Optimization, by Zhexin Xu, Nikolas R. Sanderson, Hanna Jiamei Zhang, and David M. Rosen.
• Low-Degree Implied Equalities for Strengthening Semidefinite Relaxations, by Alexandre Amice, Bernhard Paus Graesdal, Russ Tedrake, and Pablo A. Parrilo.
• A Generalized Theorem of the Alternative for Certifiable Optimization with Redundant Constraints, by Hanna Jiamei Zhang, Alan Papalia, Michael Everett, and David M. Rosen.
• Tightening Mixed-Integer Convex Relaxations for Efficient Temporal Logic Motion Planning via Logic Network Flow, by Xuan Lin, Jiming Ren, Yandong Luo, Weijun Xie, and Ye Zhao.
• Global Solvers for 3D Vision: Foundations, Frontiers, and a Call to the Robotics Community, by Zhenjun Zhao and Javier Civera.

GTSAM preview

I am exited to announce that soon GTSAM will support certifiable estimation in a big way, based on the recent QP and QCQP support in GTSAM. QCQPs are a natural bridge between factor-graph models and many convex-relaxation pipelines: once a nonconvex estimation problem is lifted into a quadratically constrained quadratic form, semidefinite relaxations and certificates can be handled ina systematic way. Keep watching this space !

Further browsing

• Frontiers of Optimization for Robotics workshop
• Accepted contributions
• Arxiv: Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs
• Post: Last week’s post on CMC-Opt and legged estimators
• Post: Quadratic Programs and QCQPs in GTSAM

Disclosure: AI was used to help draft this post and prepare the figures.

On June 1, I will be presenting CMC-Opt, new work by Yetong Zhang on constraint manifolds with corners, at the Frontiers of Optimization for Robotics workshop at ICRA. The paper is now available on arXiv:

CMC-Opt: Constraint Manifold with Corners for Inequality-Constrained Optimization, by Yetong Zhang and Frank Dellaert.

The paper presents a new way to use manifold optimization to solve large inequality-constrained optimization problems, such as state estimation and motion planning in legged systems: think quadrupeds and humanoids.

Speaking of legged systems, I’m also excited to announce more details and extensive experimental results on the four simple proprioceptive estimators for legged robots that we recently added to GTSAM. This work was done in collaboration with my recently graduated Ph.D. student Varun Agrawal, and with the awesome Chiyun Noh and Ayoung Kim from the RPM Robotics Lab at Seoul National University (SNU):

Four Simple Proprioceptive Estimators for Legged Robots by Frank Dellaert, Chiyun Noh, Varun Agrawal, and Ayoung Kim.

What these two papers have in common is that we turn the many constraints in legged systems (and robotic systems in general) into factor graphs that can be optimized to do both perception and planning. Some might say it’s sense-think-act with factor graphs.

CMC-Opt at ICRA

The core idea in CMC-Opt is to transform the hard problem of constrained optimization into an unconstrained problem directly on the feasible state space. Yetong Zhang, who is now on the motion planning team at Waymo, did a Ph.D. thesis with me on using geometry to make constrained robotic inference and planning problems easier to solve. The figure above illustrates constraint manifolds with corners, which can capture nonlinear equalities and inequalities.

Instead of using a monolithic classical constraint solver, his idea was to use the sparse graph structure and manifold optimization (as GTSAM provides) to create new manifolds out of the original variables and the constraints that involve them. Yetong created an algorithm to automatically identify “constraint-connected components” and turn them into new manifold types: lower-dimensional feasible spaces that reflect the robot’s inherent topological structure. The result is a new, coarser factor graph, which can really pay off in kinodynamic motion planning. For example, in the quadruped jumping example illustrated above, the search space drops from 32,194 dimensions to 2,260, and constraints will be satisfied by construction.

This is a continuation of the CM-Opt work that previously appeared at ICRA 2023 as Constraint Manifolds for Robotic Inference and Planning.

Legged robot estimators

The second paper is just as exciting! In a previous post, Legged State Estimation, I wrote about four simple proprioceptive estimators for legged robots, i.e., they only use an IMU and internal joint angles over time to determine the trajectory of the robot. They are basically KISS-style versions of the ideas first explored by Michael Bloesch et al. and Ross Hartley et al., and our own Humanoids 2022 publication, where footholds are treated as landmarks: foot SLAM, so to speak.

Varun Agrawal was instrumental in creating legged-robot locomotion and estimation capabilities in the same factor-graph style used elsewhere in GTSAM, and those four estimators were directly inspired by his work. All four variants are available in GTSAM. But the question remained: how well do they perform on real, hardcore data?

Enter the collaboration with Chiyun Noh and Ayoung Kim at Seoul National University (SNU): their group has done amazing work in LiDAR- and radar-based state estimation and was kind enough to help us properly evaluate our new estimators, using the data they painstakingly collected with a Boston Dynamics Spot robot to create the GaRLILEO dataset. And, to top it off, Chiyun created a ROS2-compatible implementation, so you can try it yourself!

The main discovery, hinted at in the figure above: the simple GTSAM legged estimators are not half bad, one might even say they are pretty good! In this dataset they beat out all of the state-of-the-art estimators that we tested them against. This is absolutely not the end of the story: we are working to test these estimators in several other environments and against other recent estimators. But for something that originated as an example in my advanced robotics class, they are at the very least very good baselines to test against.

Of course, these “blind” estimators are just the basis for a practical robot perception system. You need to fuse these with an external sensor, such as vision, LiDAR, or radar, which is exactly what Ayoung Kim’s group excels at doing. But it’s good to start from an estimator that at least keeps very good track of where you walk while closing your eyes!

Further browsing

• Frontiers of Optimization for Robotics workshop
• arXiv link: CMC-Opt: Constraint Manifold with Corners for Inequality-Constrained Optimization.
• blog post: Legged State Estimation
• arXiv link: Four Simple Proprioceptive Estimators for Legged Robots.
• STAG: sense-think-act with factor graphs

Disclosure: AI was used to help draft this post and prepare the figures.

In modern robotics, we often represent trajectories using discrete-time elements, but many real-world scenarios benefit from a continuous representation. Whether you are dealing with high-rate asynchronous sensors, rolling-shutter cameras, or the need to sample a trajectory at arbitrary times for control and planning, continuous-time (CT) estimation provides a principled solution.

The two main approaches for CT estimation to date involve either parametric approaches, such as splines, or non-parametric ones, such as Gaussian processes (GPs). While GTSAM is traditionally used for discrete-time factor graph problems, it now features powerful capabilities for GP-based continuous-time estimation, accompanying our recent paper.

An example of GP-based CT trajectory is shown below. The trajectory specifically includes states $\mathbf{x}$ (e.g., position or pose of a robot over time) at discrete times $t_k$ (typically when measurements occur), but retains the ability to query the mean and covariance of the state at any arbitrary time $\tau$ through the interpolation capabilities of the underlying Gaussian process.

A core insight that we share in our recent paper is that querying such a trajectory at any time is mathematically equivalent to an application of the standard factor-graph elimination algorithm. This perspective not only makes the math more intuitive but also reveals how to perform efficient $O(1)$ interpolation for both the state’s mean and covariance.

GTSAM now features built-in capabilities to carry out GP-based continuous-time estimation within the existing factor graph framework.

New GTSAM Features

We introduce several key components to GTSAM for performing CT estimation on vector spaces (Point1, Point2, Point3) and Lie groups (Pose2, Pose3):

• StateData: A custom struct that links poses, velocities, and timestamps for a given state.
• WnoaMotionFactor: A binary motion prior factor that connects neighboring pose and velocity pairs based on a White-Noise-on-Acceleration (WNOA) model. This prior smooths the trajectory by assuming constant velocities and endows the estimation with a continuous-time GP interpretation.
• WnoaInterpFactor: A powerful wrapper factor that allows you to add measurements to your graph at arbitrary timestamps, even those not included in the main graph. It internally handles the GP-interpolation, linking asynchronous data to the discrete states you are optimizing.
• interpolateFactorGraph: A convenience function that can automatically convert a standard factor graph into a reduced, equivalent graph where selected states are interpolated, significantly reducing the size of the optimization problem.
• Post-Solve Querying: Functions like updateInterpValues and updateInterpValuesWithCovariance allow you to query the full, smooth trajectory and its uncertainty after the main optimization is complete.

An Example on $SE(3)$

To see these tools in action, let’s consider a simple $SE(3)$ trajectory example.

The most straightforward workflow is to define a trajectory by creating StateData entries for the states at discrete times and linking them with the binary WnoaMotionFactor. We can then add measurements to any of those states at discrete times, such as pose or velocity observations. In this example, we use pose measurements. The resulting graph contains both binary and unary factors, and solving it with standard GTSAM solvers gives us the mean and covariance of each state, as shown in the figure below.

We can now use the new GTSAM functionalities to additionally query the mean and covariance of the trajectory at any arbitrary time. The figure below shows a much denser trajectory with a smooth state. States at queried times can be recovered by drawing an analogy to factor-graph elimination, as illustrated in the figure. This lets us query the trajectory after solving the discrete problem at any time, which is useful for downstream tasks such as planning and control.

Finally, we are not restricted to measurements, or any other factors, that align exactly with the optimization states at discrete times.

As illustrated in the figure below, suppose we receive a measurement at an arbitrary time $\tau$. In a traditional setup (left), we would need to insert a new state $\mathbf{x}_\tau$ into the optimization graph. Instead, we can conceptually eliminate $\mathbf{x}_\tau$ using the GP motion model. This elimination produces two distinct components (right):

1. A conditional density $p(\mathbf{x}_\tau \mid \mathbf{x}_k, \mathbf{x}_{k-1})$ representing the eliminated state $\mathbf{x}_\tau$ (shown in gray).
2. A new measurement factor $\phi_y(\mathbf{x}_{k-1}, \mathbf{x}_k)$ that depends solely on the two neighboring bounding states, $\mathbf{x}_{k-1}$ and $\mathbf{x}_k$.

In GTSAM, we implement this mechanism using the wrapper factor WnoaInterpFactor. It converts a factor at an arbitrary time into this neighboring-state factor, allowing GTSAM to optimize the trajectory while correctly accounting for exact time associations via GP interpolation.

The bottom of the figure demonstrates this in practice. This formulation is especially useful when measurements are asynchronous or arrive at high rates (shown in red). Rather than including states in the graph for every single measurement, we only need to optimize a subset of discrete states (dark blue) at a lower rate, such as 10 Hz. The wrapper factor relies on GP interpolation to correctly relate the asynchronous measurements to the discrete states, effectively enabling exact continuous-time interpolation during the solve rather than only post-solve.

Takeaway

The integration of GP-based continuous-time estimation into GTSAM provides a flexible, modular framework that maintains the library’s standard workflow. It allows practitioners to handle asynchronous sensor data in a principled way and reduce problem sizes without sacrificing the fidelity of the continuous trajectory.

Additional Reading

For a deeper dive into the theory and broader context of these methods, we recommend reading our recent paper, “Smoothing Out the Edges: Continuous-Time Estimation with Gaussian Process Motion Priors on Factor Graphs”.

You can find more details on the example used in this blogpost in our GTSAM Example Notebook and interactive Google Colab Notebook. Please also visit our GitHub Repository for further real-world robotics examples. These specifically include:

1. 1D (Giant Glass of Milk): A mobile robot driving back and forth in a straight line, demonstrating basic smoothing.
2. 2D (Lost in the Woods): SLAM and localization for a wheeled robot using asynchronous landmark observations, showing problem-size reduction via aggressive interpolation.
3. 3D (Starry Night): $SE(3)$ localization using a stereo camera and IMU, illustrating high-fidelity trajectory recovery with 80% fewer states in the main solve by relying on GP interpolation.

Disclosure: AI was used to help draft this post.

Quadratic programming is one of the quiet workhorses behind modern robot motion. In legged robot locomotion, QPs show up in contact-force allocation, whole-body control, and model-predictive control loops that have to respect friction, torque, and unilateral-contact constraints. In quadrotor flight, small QPs appear whenever a desired thrust vector has to be mapped onto bounded rotor speeds. These are everyday constrained optimization problems that many GTSAM users already solve somewhere else in their stack.

GTSAM now has first-class building blocks for QP and QCQP modeling in the constrained module. The new pieces let you express quadratic objectives, linear constraints, and quadratic constraints over direct Vector and Matrix entries in Values, while still living inside GTSAM’s factor-graph-flavored ecosystem. There is also LP support, which is useful and closely related, but less central to the typical GTSAM user. This post therefore focuses on QP and QCQP.

QP: Quadratic objectives with linear constraints

A QP optimizes a quadratic objective subject to linear equalities or inequalities. In GTSAM terms, that means you can combine a quadratic cost, such as one coming from a HessianFactor or QpCost, with constraints like A x = b, A x <= b, or A x >= b. Geometrically, the optimizer is looking for the lowest point of a bowl after slicing it by planes and half-spaces. The active constraints are the boundaries that actually determine the final solution.

The two-dimensional QP example above makes these active constraints visible. The yellow point is the unconstrained target, the dashed line is an equality constraint, and the green segment is the part of that line that survives the inequalities. The optimizer yields the best target-compatible point that still satisfies all constraints. This same model describes many robotics QPs, even when the variables are contact forces, accelerations, or actuator commands rather than two plotted coordinates.

The quadrotor allocation example above shows why small QPs matter in real-time systems. Given a desired thrust and body torque, the allocation matrix maps four normalized rotor thrusts into a wrench, while box constraints keep each rotor inside its physical range. The unconstrained request would push one rotor beyond its limit, so the constrained solution saturates that rotor and distributes the remaining effort across the others. This is a tiny, fixed-structure problem where warm starts and solver overhead matter.

In terms of solves, both LP and QP problems have a sparse active-set solver, which is the natural starting point for chains, grids, and SLAM-like structures, where sparsity matters. QP also has a dense active-set path for tiny warm-started fixed-structure problems, such as the control-allocation QP above. There dense linear algebra beats graph setup costs. Because QPs are constrained optimization problems, they can also be handed to any constrained optimizer, such as the augmented Lagrangian optimizer.

QCQP: Quadratic constraints

A QCQP keeps the quadratic objective but allows the constraints themselves to be quadratic. Instead of only carving a bowl with lines or planes, a QCQP can impose boundaries such as ellipses, spheres, norm bounds, and other scalar quadratic relations. That small change is important because many robotics constraints are naturally quadratic, e.g., “being on the $SO(3)$ manifold” can be expressed as a set of quadratic constraints.

The QCQP geometry example above shows how much changes when constraints can curve. The target lies outside the feasible region, but the feasible region is no longer a polygonal slice of the plane. One quadratic constraint creates the circular boundary, while another bounds the squared vertical coordinate. The solution sits where those two active quadratic constraints meet. This is still an optimization problem over familiar Values, but the feasible set has become rich enough to express a much broader family of robotics problems.

In particular, QCQP is a common language for convex relaxation and certifiable state estimation. Many estimation problems that start as non-convex geometry can be lifted, relaxed, or bounded through quadratic objectives and quadratic constraints, and those formulations are often the bridge to certificates of global optimality. That is the exciting part we are only hinting at today: future posts will go deeper into how these ideas connect to certifiable estimators in GTSAM. Stay tuned !

Takeaway

The takeaway is that GTSAM’s constrained module now reaches beyond nonlinear least squares into optimization models robotics users already rely on. QP gives you a standard way to model constrained quadratic objectives, QCQP adds quadratic feasible sets, and the docs and notebooks (see below) provide runnable examples without requiring a separate optimization package just to get started. LP, QP, and QCQP open a path toward richer constrained estimation, control, and certifiable inference inside GTSAM.

Additional Reading

The practical entry points are the notebooks and docs rather than a long API tour here. The QpProblem docs describe the core QP model, and the runnable QP example notebook shows both a two-dimensional geometry example and a quadrotor allocation example. The corresponding QcqpProblem docs and QCQP example notebook cover quadratic constraints. If you want the linear-programming sibling, the LP example notebook is the right place to start.

Disclosure: AI was used to help draft this post and prepare the figures.

In Part 3 of the Manifold Filter Hierarchy, we introduced the EquivariantFilter template in GTSAM and walked through the geometry that makes EqF useful: a state on a manifold $\mathcal{M}$, a symmetry group $\mathcal{G}$ acting on it, and an error expressed around a fixed reference state instead of the current estimate. In that post, we discussed a very simple toy problem of an attitude-on-a-sphere.

In today’s post, we consider more complex problems which can be tackled using an EqF, such as VIO (Visual-Inertial-Odometry) and ABC (Attitude-Bias-Calibration). In tandem, we also announce AwesomeEqF: a community-curated collection of papers and runnable notebooks for equivariant filtering, built around GTSAM.

What is AwesomeEqF?

AwesomeEqF is a website (and repo) containing:

• A reading list of papers, organized from foundational invariant-observer papers through to recent EqF architectures
• A growing set of notebooks that utilize GTSAM’s EqF filter implementation and perform on real data
• A soon-to-be blog for tutorials and write-ups that contextualize specific papers.

The intent is for AwesomeEqF to be the place a roboticist lands when they have read about the potential of equivariant filtering and are excited to get their hands dirty. Contributions are welcome, see the Contributing Guide.

A Quick Recap of the EqF

From Part 3: an EqF stores the estimate as a fixed reference state $\xi^\circ \in \mathcal{M}$ together with a group element $\hat{g} \in \mathcal{G}$. The current estimate is recovered by the group action $\hat{\xi}$, and the natural error $e$ lands at $\xi^\circ$ when the filter is correct.

Because the linearization happens around that fixed reference rather than the moving estimate, covariance propagation depends much less on whether the current guess is right.

Every new EqF needs two problem-specific ingredients before it can run like an ordinary EKF. We need to supply the lift that turns physical dynamics into a small motion in the group, and the equivariance conditions that the dynamics and outputs have to satisfy. Once those are in, the actual runtime loop looks like an ordinary EKF on the group tangent space.

The rest of this post applies that EqF pattern to two practical cases: visual-inertial odometry (VIO) and attitude estimation.

The EqVIO: Equivariant Visual-Inertial Odometry

The EqVIO filter by Pieter van Goor and Robert Mahony is the answer to a long-standing complaint about EKF-based VIO: standard filters such as MSCKF (Multi-State Constraint Kalman Filter) accumulate inconsistency because the linearization point drifts with the (possibly wrong) estimate, and the filter ends up more confident than its actual error warrants. The EqVIO removes that source of inconsistency by construction, using equivariance.

The VIO state

The EqVIO represents VIO with a manifold state containing pose, velocity, IMU biases, camera extrinsics, and tracked landmarks. At every timestep, its physical state is composed of:

1. body pose $P \in SE(3)$,
2. body linear velocity $v \in \mathbb{R}^3$,
3. gyroscope bias $b_\omega \in \mathbb{R}^3$,
4. accelerometer bias $b_a \in \mathbb{R}^3$,
5. camera-to-IMU rigid offset $T_{ci} \in SE(3)$,
6. a set of 3D landmarks $p_i \in \mathbb{R}^3$ corresponding to tracked image features.

Note that the state is not a Lie group, but rather a manifold, which is exactly the situation an EqF is good for.

The symmetry group

The key construction in the EqVIO is a symmetry group $\mathcal{G}$ that acts on the full composite VIO state. The group is built as a product Lie group whose factors mirror the state itself:

Each group factor corresponds to a specific part of the VIO state:

• $SE_2(3)$, the “extended pose” group, jointly handles attitude, position, and velocity.
• $\mathbb{R}^6$ for the two IMU biases, which the semi-direct product couples back into the inertial frame.
• $SE(3)$ for the camera offset $T_{ci}$, so online extrinsic refinement is part of the geometry.
• $\mathrm{SOT}(3)$ (rotation plus scaling) per landmark, capturing the rotation-and-depth ambiguity that visual measurements really do have.

The action $\phi$ of $\mathcal{G}$ on the state is then the natural one inherited from each factor.

$\mathrm{SOT}(3)$ is the rotation-and-positive-scale group that the EqVIO uses to encode each landmark’s monocular bearing-depth ambiguity. Some readers may be unfamiliar with this group, as it is not a commonly defined group in GTSAM, but rather specific to the EqVIO implementation. Concretely,

is the direct product of a rotation $R \in SO(3)$ and a strictly positive scalar $s \in \mathbb{R}_{>0}$, acting on a 3D point as $q \mapsto s\, R\, q$. This group matters because monocular projection only fixes a landmark up to its bearing direction and a positive depth scaling, which is exactly the ambiguity that $\mathrm{SOT}(3)$ encodes, so making it part of the symmetry is what lets the equivariant output approximation in the next section work.

The equivariant output approximation

Camera measurements are “equivariant” when transforming the state produces a predictable transformation of the measurement.

EqVIO takes advantage of the equivariant output approximation, a general EqF construction that is available when the output map is equivariant. As is classic for VIO systems, the camera observes a landmark and produces a bearing vector. In the paper, van Goor and Mahony show that the landmark bearing measurement is equivariant with respect to the SOT(3) symmetry, which enables this same approximation in the camera update.

The equivariant output approximation can reduce camera measurement approximation error by an order of magnitude. Traditionally, EKFs use a first-order approximation of the measurement function; by exploiting the equivariance property, we can use the group structure to process input camera data with a more faithful local approximation.

The same symmetry argument also motivates inverse-depth-style landmark parameterizations. It is well known that representing landmarks by inverse distance can improve stability of a filter. The authors present a polar parameterization derived from $\mathrm{SOT}(3)$ that can act similar to inverse-depth but also further minimize linearization error. Currently, this polar parameterization is not implemented into GTSAM’s EqVIO implementation, but it is a near-future change on the roadmap.

What the runtime loop looks like

The EqVIO runtime loop has the familiar predict-update shape, with extra feature management for a changing landmark set:

1. Predict. IMU frames drive a small motion in the group via the lift.
2. Update. For each tracked feature in a new image frame, predict the normalized image coordinate, compare to the observation, form the equivariant innovation, and apply a correction in the tangent space.
3. Manage features. Add new landmarks for unseen features, drop features that fall out of the image, and keep the covariance block consistent with the live landmark set.

Feature management is essential bookkeeping for running the EqVIO filter on real sequences without the state vector blowing up. This bookkeeping step is new compared with the smaller examples in the previous posts.

The EqVIO in GTSAM

GTSAM exposes the EqVIO through three implementation files in gtsam_unstable/navigation:

• EqVIOState.h: the manifold state described above, including the dynamic landmark list.
• EqVIOSymmetry.h: the semi-direct-product group $\mathcal{G}$, its action on EqVIOState, and the lift used during prediction.
• EqVIOFilter.h: the filter class and the predict / update entry points.

EqVIOFilter is built on the EquivariantFilter template that Part 3 introduced. xi_ref_ is the EqVIO reference state, and g_ is the lifted group estimate that the IMU drives.

Python users can access the same EqVIO filter through gtsam_unstable.eqvio bindings that closely mirror the C++ API. The snippet below walks through the initialization and propagation of the filter, which is described in more detail in the AwesomeEqF notebook!

import gtsam, gtsam_unstable

xi_ref = gtsam_unstable.eqvio.State()
xi_ref.cameraOffset = T_ci
params = gtsam_unstable.eqvio.EqVIOFilterParams()
filter = gtsam_unstable.eqvio.EqVIOFilter(
    xi_ref, initial_covariance, gtsam.KeyVector(), params
)

filter.initializeFromIMU(first_imu)
for imu_input, dt in imu_stream:
    filter.predict(imu_input, dt)
for features, R in vision_stream:  # features: dict[int, np.ndarray(2,)]
    filter.update(features, camera, R)

The full Python walkthrough lives here, as a notebook on AwesomeEqF. Take a look!

This paper-to-code-to-notebook path is the main reason AwesomeEqF exists. Equivariant filtering papers often introduce useful geometry, but it can be hard to see how that geometry turns into a working estimator on data. AwesomeEqF is meant to close that gap by pairing the paper trail with GTSAM implementations and runnable examples, so a reader can move from the idea to a filter they can inspect, modify, and run.

The ABC-EqF: Attitude, Bias, Calibration

We also added a second example on the website. The “ABC-EqF” applies the same equivariant-filter design idea to attitude estimation with bias and calibration. The filter comes from Fornasier, Ng, Brommer, Böhm, Mahony, and Weiss, whose paper covers equivariant filter design for attitude state estimation.

The ABC-EqF state stacks three attitude-estimation quantities:

• attitude $R \in SO(3)$,
• gyroscope bias $b \in \mathbb{R}^3$,
• and a sensor calibration rotation $C \in SO(3)$ that aligns a reference direction sensor (e.g. magnetometer) with the body frame

Note that the formulation generalizes to $N$ direction sensors with $n \leq N$ calibration states, so if there are more than 1 uncalibrated sensor, the last item in the state would be $SO(3)^n$.

The classical EKF treats bias and calibration as ordinary linear states tacked onto attitude, but the ABC-EqF instead uses the symmetry group

so the attitude and gyro-bias live in a coupled semi-direct-product geometry (the first two terms) while the calibration rotation carries its own $SO(3)$ factor, where $n$ is the number of sensors with extrinsic calibration states to be estimated. The result is better linearization behavior, faster bias convergence, and an estimator whose consistency does not depend on the bias estimate being good.

In GTSAM this is implemented as the ABCEquivariantFilter in gtsam_unstable; there, the $(SO(3) \ltimes \mathfrak{so}(3))$ factor is represented using Pose3, leveraging the Lie-group isomorphism $(SO(3) \ltimes \mathfrak{so}(3)) \cong SE(3)$.

A complete C++ example lives at AbcEquivariantFilterExample.cpp, and the AwesomeEqF notebook here walks through that code in Python with interactive plots for the attitude, bias, and calibration errors over time.

Takeaway

The EqVIO and the ABC-EqF show that the EqF machinery from Part 3 now reaches practical robotics estimators in GTSAM. The EqVIO filter is the most recent application, and it is now usable from Python through GTSAM.

AwesomeEqF is meant to grow with the field. If you have a paper, a notebook, or a write-up that fits, please open a pull request.

Further Reading

• “EqVIO: An Equivariant Filter for Visual Inertial Odometry”, van Goor and Mahony.
• “Overcoming Bias: Equivariant Filter Design for Biased Attitude Estimation with Online Calibration”, Fornasier, Ng, Brommer, Böhm, Mahony, and Weiss.
• “Equivariant Symmetries for Aided Inertial Navigation”, Fornasier (dissertation).
• AwesomeEqF site: borglab.github.io/AwesomeEqF.
• GTSAM EqVIO source: gtsam_unstable/navigation/EqVIOFilter.h.
• AwesomeEqF EqVIO notebook: EqVIO Example.
• AwesomeEqF ABC-EqF notebook: ABC-EqF Example.

Last week we zoomed out and talked about STAG: state, dynamics, measurements, objectives, and how factor graphs can tie those pieces together. We discussed fixed-lag smoothing, which optimizes over a short window of recent states using measurement factors. This week we zoom back in on single-state filters, specifically filters in which the state lives on a manifold.

In GTSAM, we now also provide an equivariant filter. The word equivariant has the same basic meaning here as it does in equivariant neural networks: if you transform the input, the output should transform in the corresponding way. In filtering, the payoff is not just elegance. If the dynamics and measurements respect the symmetry, the filter can express its error around a fixed reference state instead of around the current, possibly wrong, estimate.

This post is meant as a quick tutorial introduction for GTSAM users. The foundations were developed by Mahony, Hamel, and Trumpf in “Equivariant Systems Theory and Observer Design”, and the main EqF reference is “Equivariant Filter (EqF)” by van Goor, Hamel, and Mahony. An earlier short paper, “Equivariant Filter Design for Kinematic Systems on Lie Groups” by Mahony and Trumpf, is also useful background.

EqF: the Missing Piece in the Hierarchy

This is the third post in the manifold Kalman filter hierarchy series. Part 1 introduced ManifoldEKF, LieGroupEKF, InvariantEKF, and LeftLinearEKF; Part 2 showed why invariant filtering matters in legged state estimation.

The Equivariant Filter (EqF) is the final missing piece. The ManifoldEKF can handle a broad range of manifold states, including Unit3, but its covariance propagation is tied to the current estimate because the transition Jacobian is computed in the tangent space at the current linearization point. A bad estimate can therefore give you a bad local error model. The InvariantEKF and LeftLinearEKF get the invariant-error behavior we like, where the propagated error can be much less dependent on the current estimate, but they assume the state itself is a Lie group.

The new EquivariantFilter fills the gap: it is an error-state Kalman filter where the physical state can live on a general manifold $\mathcal{M}$, while a separate symmetry group $\mathcal{G}$ acts on that state. The group action gives the filter an invariant-style error without requiring the state itself to be a group.

A Simple Example

Imagine a robot carrying a gyroscope and a magnetometer. We want to estimate the direction of the magnetic field in the robot’s body frame. That direction is a unit vector

with dynamics

and measurement

Here, $\Omega$ is the angular velocity measured by the gyroscope. The matrix $\Omega^\times$ is the cross-product matrix, so the dynamics say that the magnetic-field direction appears to rotate in the robot’s body frame as the robot turns. The measurement equation says the magnetometer observes that same direction, scaled by a constant magnetic-field strength $c_m$.

The important detail is that the state is not a group. It is a point on the sphere:

In GTSAM, that is a Unit3. You can perturb it locally with two tangent coordinates, but you cannot compose two directions and get another direction in the group-theoretic sense. So this is a natural state for ManifoldEKF, not for InvariantEKF.

However, rotations act on directions. The group

moves points around on $\mathbb{S}^2$. In GTSAM terms, the physical state can be Unit3, while the symmetry group can be Rot3. This is exactly the kind of situation EqF is designed for: the state is not a group, but a group still acts on it.

The EqF Idea

There are three geometric objects in play. The physical state lives on a manifold $\mathcal{M}$, the symmetry lives in a group $\mathcal{G}$, and the group action

tells us how a group element moves a state. For the sphere example, the state is a direction $\eta \in \mathbb{S}^2$, the group is $SO(3)$, and the action rotates the direction. This is the extra structure beyond ManifoldEKF: the state does not have to be a group, but there must be a useful group action on it.

Instead of storing the estimate only as a state $\hat{\xi}$ on the manifold, the EqF also stores a group element $\hat{g}$ and applies it to a fixed reference state $\xi^\circ$:

In the sphere example, $\xi^\circ$ might be the north pole direction, and $\hat{g}$ is a rotation that moves that reference direction to the current estimate.

In the GTSAM implementation, the physical estimate is still an element of M, but EqF also maintains a group element g_; applying g_ to xi_ref_ recovers the current state estimate.

This also gives a natural error:

If the estimate is correct, the error lands back at the fixed reference point:

That fixed reference point is the key. In a usual manifold EKF, the tangent space and linearization move with the current estimate. In an equivariant filter, the symmetry action lets us express the error around a fixed origin. Equivariance is the consistency condition that makes this legitimate: moving the state by the symmetry and then applying the dynamics or measurement model must match applying the model first and then moving the result in the corresponding way. When that holds, covariance propagation is less tied to the filter’s current guess.

The runtime loop still looks like an EKF: predict the state, propagate covariance, compare predicted and observed measurements, compute a Kalman gain, and apply a correction. The difference is where the geometry enters. During prediction, the lift $\Lambda$ converts physical dynamics into a small motion in the group. In the attitude-direction example, the gyroscope moves the group estimate, and the direction estimate follows from the group action. During update, the Kalman correction is lifted back through the group instead of directly nudging arbitrary coordinates on the sphere.

The GTSAM Template

The generic implementation lives in gtsam/navigation/EquivariantFilter.h. It is templated on $\mathcal{M}$, the physical manifold state type, and Symmetry, the functor that defines the group action. The class inherits from ManifoldEKF<M>, so it reuses the same covariance, Kalman gain, and Joseph-form update machinery from the base hierarchy.

Importantly, the covariance has the dimension of M, not the dimension of Symmetry::Group: the group is not replacing the physical state with a larger Kalman state. It is used to transport the estimate and corrections in a way that respects the symmetry. The EqF-specific part is the additional symmetry bookkeeping:

• Symmetry::Group is the internal group $\mathcal{G}$
• xi_ref_ is the fixed reference state $\xi^\circ$
• g_ is the lifted group estimate
• act_on_ref_(g_) recovers the current manifold estimate
• Dphi0_ maps group tangent perturbations down to manifold tangent perturbations
• InnovationLift_ maps the EKF innovation back up to the group tangent space

So, you need to provide more structure than a plain ManifoldEKF: the physical state type, the Symmetry action, the lift from physical dynamics to group motion, the input and output actions needed for equivariance, and the usual process and measurement noise models. The payoff is that the filter is using the geometry of the problem rather than an arbitrary local coordinate choice.

The EqF is worth considering when all three of these are true:

1. Your state lives naturally on a manifold that is not necessarily a group.
2. A Lie group acts on that state in a meaningful way.
3. You care about the invariant-filter benefit: error and covariance propagation that are less dependent on the current estimate being right.

If the state is just a generic manifold with no useful symmetry, ManifoldEKF is the right abstraction. If the state is a Lie group and the dynamics have the invariant form, InvariantEKF or LeftLinearEKF might be simpler.

Takeaway

The EquivariantFilter is a natural next step after the hierarchy in Part 1 and the invariant-filter application in Part 2. We now have a better answer for states such as directions on a sphere: keep the state on the physical manifold, but let a symmetry group push that state around. That gives GTSAM users a way to model states that are not groups without giving up on invariant filtering.

Further Reading

• “Equivariant Systems Theory and Observer Design”, Mahony, Hamel, and Trumpf
• “Equivariant Filter (EqF)”, van Goor, Hamel, and Mahony
• “Equivariant Filter Design for Kinematic Systems on Lie Groups”, Mahony and Trumpf
• “Overcoming Bias: Equivariant Filter Design for Biased Attitude Estimation with Online Calibration”, Fornasier, Ng, Brommer, Böhm, Mahony, and Weiss. This is the ABC paper that directly inspired the GTSAM ABC example.
• GTSAM EqF test: testEquivariantFilter.cpp
• A more complicated ABC EqF example in GTSAM: Example, Filter

Disclosure: AI was used to help draft this post.