⚡ electrical engineering

A Smart-Scheduled Hybrid (SSH) EKF-FGO State Estimation

This paper experimentally characterizes optimization scheduling as a critical, independent design variable in a Smart-Scheduled Hybrid (SSH) EKF-FGO framework, demonstrating that strategically timing batch optimization can significantly reduce computational costs while retaining most of the global consistency benefits in state estimation.

Original authors: Eric Levi, Soosan Beheshti

Published 2026-06-16

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Eric Levi, Soosan Beheshti

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to navigate a robot through a maze. To do this, the robot needs to constantly guess where it is. This paper is about finding the perfect balance between guessing quickly and guessing correctly, without exhausting the robot's brainpower.

Here is the breakdown of the problem and the solution, using simple analogies:

The Problem: Two Ways to Navigate

The paper compares two ways a robot can figure out its location:

The "Fast Runner" (EKF): Imagine a runner who takes quick, confident steps. They update their position every second. It's very fast and uses little energy. However, because they don't look back to check their map, they slowly start to drift off course. Over time, a small error in every step adds up, and they end up far from where they actually are.
The "Slow Map-Reader" (FGO): Imagine a person who stops every few minutes to pull out a giant map, look at every landmark, and calculate exactly where they are relative to the whole world. This is incredibly accurate and fixes all the previous mistakes. But, it takes a long time and uses a lot of mental energy. If they did this every second, they would never move.

The Solution: The "Smart Scheduler"

The authors created a hybrid system called SSH (Smart Scheduled Hybrid). Think of this as a team with a Fast Runner and a Map-Reader working together, but with a strict rulebook on when the Map-Reader gets to speak.

The Routine: The Fast Runner (EKF) keeps moving and updating the position every single second. This keeps the robot moving smoothly and efficiently.
The Check-in: Instead of checking the map every second (too slow) or only at the very end (too much drift), the system uses a Schedule. Every $W$ seconds (the "Scheduling Interval"), the system pauses, calls in the Map-Reader (FGO), and corrects the Fast Runner's accumulated mistakes.
The Reset: Once the Map-Reader fixes the position, the Fast Runner resets its internal compass to this new, accurate spot and starts running again.

The Big Discovery: The "Asymmetric Trade-off"

The paper's main finding is a surprising relationship between how often they check the map and how much it costs.

Imagine you are driving a car and checking your GPS.

If you check the GPS every 10 seconds: You are very accurate, but you are constantly distracted, and it uses a lot of battery.
If you check the GPS every 100 seconds: You save a massive amount of battery (computational cost), and you don't get distracted often.

The paper found that the cost drops like a stone, but the error grows like a gentle hill.

Cost: If you stop checking the map as often, your computer workload drops drastically (you save huge amounts of energy).
Error: If you stop checking the map as often, your position only gets slightly worse, not catastrophically worse.

The "Sweet Spot":
The researchers found a "Goldilocks zone" (specifically checking every 10 to 50 steps). In this zone, the robot saves 90% of the computer power but still keeps the "drift" (the error) extremely low—about 85-90% better than just guessing without ever checking a map.

What They Tested

They tested this in a computer simulation where a robot drove in a perfect circle. They tried different schedules:

Checking every 2 steps.
Checking every 100 steps.
Checking every 400 steps.

The Results:

Even when they checked the map very rarely (every 400 steps), the robot was still 3 to 4 times more accurate than a robot that never checked the map at all.
The final destination was almost exactly the same whether they checked the map often or rarely, as long as they checked it at least once at the end.
The biggest benefit was in the middle of the journey: The robot stayed much closer to the true path without needing to do heavy math constantly.

The Takeaway

The paper concludes that timing is everything. By treating "when to check the map" as a specific dial you can turn, engineers can get the best of both worlds: the speed of the Fast Runner and the accuracy of the Map-Reader, without paying the full price for either. It turns out you don't need to check the map constantly to stay on track; you just need to check it at the right intervals.

Technical Summary: A Smart-Scheduled Hybrid EKF–FGO State Estimation

Problem Statement
Reliable state estimation in robotics, particularly within Simultaneous Localisation and Mapping (SLAM) pipelines, necessitates a trade-off between estimation accuracy and computational cost. Filtering-based methods, such as the Extended Kalman Filter (EKF), offer efficient real-time updates but suffer from linearization errors that lead to estimator inconsistency and the accumulation of drift over time. Conversely, optimisation-based approaches using Factor Graph Optimisation (FGO) improve global consistency by jointly refining multiple states but incur significantly higher computational demands. While the characteristics of these individual methods are well understood, the temporal organisation of their combination—specifically, the scheduling of when global optimisation should be invoked—has received limited systematic analysis. In practice, optimisation scheduling is often treated as an implicit design choice rather than an explicit, tunable variable.

Methodology
To address this gap, the authors introduce a Smart Scheduled Hybrid (SSH) EKF–FGO framework. This framework is designed to make the interaction between recursive filtering and batch optimisation explicit and experimentally controllable. The core methodology involves:

Hybrid Architecture: The system performs recursive EKF filtering at every time step to propagate state estimates. Periodically, at discrete intervals defined by a scheduling parameter $W$ , a batch optimisation is triggered.
Bidirectional Information Exchange:
- EKF to FGO: At each optimisation event, the current EKF estimate (mean $\mu_t$ and covariance $P_t$ ) is injected into the factor graph as a unary prior factor. This prior is weighted by a scaling parameter $\alpha$ , allowing the system to balance adherence to the filter's belief against the global optimisation.
- FGO to EKF: Following the batch optimisation, the EKF is reinitialised with the optimised state estimates to maintain consistency between the filtering and optimisation components.
Experimental Control: The framework fixes the solver structure (Gauss-Newton iterations) and effort to isolate the effects of the scheduling interval $W$ . The study utilizes a planar SLAM simulation with a circular trajectory to ensure consistent geometric conditions for analysing scheduling effects.

Key Contributions
The primary contribution of this work is the experimental characterisation of optimisation scheduling as a distinct design dimension in hybrid state estimation systems. Rather than proposing a new inference algorithm or solver architecture, the paper provides a controlled framework to analyse how the frequency of batch optimisation influences:

Intermediate estimation behaviour (specifically pre-optimisation drift).
Computational cost.
The trade-off space between filtering and batch optimisation.

Results
Simulation experiments comparing the SSH EKF–FGO against a pure EKF and a classical "odometry-only" FGO baseline (where propagation occurs without filtering) reveal several key findings:

Drift Reduction: The hybrid approach significantly reduces pre-optimisation drift compared to odometry-only propagation. Across scheduling intervals $W$ ranging from 2 to 50, the hybrid estimator achieves a drift reduction of approximately 80–90% relative to the baseline. Even at infrequent intervals (e.g., $W=200$ ), the drift remains three to four times lower than the baseline.
Asymmetric Trade-off: The study identifies a critical asymmetry in the system's behaviour:
- Drift: Increases gradually and monotonically as the scheduling interval $W$ increases.
- Computational Cost: Decreases rapidly as $W$ increases.
Operating Regimes: This asymmetry creates favourable operating regimes (e.g., $W \in [10, 50]$ ) where the system retains most of the drift-reduction benefits of frequent optimisation while reducing computational load by more than an order of magnitude.
Final Accuracy: Final Absolute Trajectory Error (ATE) for the hybrid estimator remains nearly constant across all scheduling intervals and closely matches the accuracy of pure batch FGO, indicating that scheduling primarily affects intermediate behaviour rather than final convergence, provided at least one batch optimisation occurs.

Significance
The paper concludes that optimisation scheduling is a key, yet under-explored, design dimension in hybrid state estimation. The findings demonstrate that by explicitly treating scheduling as a tunable parameter, engineers can navigate the trade-off between estimation accuracy and computational efficiency more effectively. The SSH framework enables the retention of FGO-level global consistency and EKF-level intermediate stability while allowing for substantial computational savings. The authors position this work not as a replacement for established incremental solvers (like iSAM2), but as a tool for understanding the fundamental dynamics of scheduling in hybrid systems, with future work planned to extend these analyses to adaptive strategies and more complex trajectories.