State Estimation via Kalman Filtering vs Factor Graph Optimization

Extended Kalman Filters (EKF) excel as a lightweight, recursive state estimator, processing sensor data sequentially with minimal computational overhead. This makes them a long-standing workhorse in robotics for fusing high-frequency IMU and joint encoder data. For example, an EKF running on a typical embedded processor can update a humanoid's base state estimate at over 1 kHz, providing the low-latency feedback critical for tight balance control loops. However, this efficiency comes at a cost: EKFs linearize non-linear dynamics and are inherently causal, making them prone to accumulating drift and struggling to reason about past states when contact events are intermittent or ambiguous.

Factor Graph Optimization (FGO) takes a fundamentally different approach by framing state estimation as a full nonlinear least-squares problem over a window of historical data. Instead of committing to a single linearization point, FGO solvers like GTSAM or G2O iteratively relinearize and resolve the entire trajectory, treating sensor measurements and motion models as probabilistic constraints. This results in significantly lower long-term drift and superior handling of sporadic contact sensors, such as foot pressure or contact switches. The key trade-off is computational cost; solving a factor graph with a sliding window of 5-10 seconds of data can require an order of magnitude more computation than an EKF update, demanding more powerful on-board compute or a real-time optimized solver.

The key trade-off: If your priority is minimal computational load, ultra-low latency for high-bandwidth control, and your contact state is consistently well-defined, an EKF remains a highly effective choice. If you prioritize minimal long-term drift, robust state recovery after intermittent contact or slippage, and have the computational budget for a batch-optimized backend, Factor Graph Optimization provides a more accurate and globally consistent state estimate for dynamic balancing.

Extended Kalman Filters (EKF) excel at high-frequency, real-time state estimation because of their computational efficiency and recursive nature. For example, an EKF running on a standard on-board computer can fuse IMU and joint encoder data at 1 kHz with a latency of under 1 millisecond, making it the workhorse for tight inner-loop balance controllers. Its strength lies in marginalizing out old states to maintain a constant-time update, which is critical when a humanoid needs to react instantly to a push. However, this marginalization permanently linearizes past measurements, causing estimator inconsistency and drift, especially during intermittent contact like a foot slipping or scuffing the ground.

Factor Graph Optimization (FGO) takes a fundamentally different approach by solving a full nonlinear least-squares problem over a window of past states. This results in significantly higher accuracy and lower long-term drift, with studies showing up to a 40% reduction in absolute trajectory error compared to EKF in legged locomotion. By relinearizing past measurements and modeling contact as binary factors, FGO handles intermittent contact and outlier sensor readings with far greater robustness. The trade-off is computational cost: a full graph optimization can take 10-50 milliseconds to solve, making it too slow for a 1 kHz control loop without careful incremental solvers like iSAM2.

The key trade-off: If your priority is ultra-low latency for a real-time balance controller and you have reliable, continuous foot contact, choose an EKF. If you prioritize long-term accuracy, robustness to foot slippage, and a unified framework for sensor fusion and calibration, choose Factor Graph Optimization. The modern consensus is a hybrid architecture: use an EKF for high-frequency state propagation and a factor graph in a parallel loop for low-frequency drift correction and global map building.