Constrained Factor Graph Optimization for Robust Networked Pedestrian Inertial Navigation

TL;DR

This work targets drift mitigation in networked pedestrian inertial navigation by embedding both equality (ZUPT) and inequality (inter-foot distance) constraints into a constrained Factor Graph Optimization framework. A differentiable softmax penalty is introduced to enforcing the inter-foot distance bound within the FGO cost, enabling stable, gradient-based optimization across multiple epochs. The method uses IMU preintegration, ZUPT factors, and a centralized fusion of two foot-mounted IMUs, solved efficiently with GTSAM and iSAM2. Real-world experiments show that the constrained FGO approach substantially improves horizontal localization accuracy over conventional constrained Kalman filters, demonstrating robustness in GNSS-denied environments.

Abstract

This paper presents a novel constrained Factor Graph Optimization (FGO)-based approach for networked inertial navigation in pedestrian localization. To effectively mitigate the drift inherent in inertial navigation solutions, we incorporate kinematic constraints directly into the nonlinear optimization framework. Specifically, we utilize equality constraints, such as Zero-Velocity Updates (ZUPTs), and inequality constraints representing the maximum allowable distance between body-mounted Inertial Measurement Units (IMUs) based on human anatomical limitations. While equality constraints are straightforwardly integrated as error factors, inequality constraints cannot be explicitly represented in standard FGO formulations. To address this, we introduce a differentiable softmax-based penalty term in the FGO cost function to enforce inequality constraints smoothly and robustly. The proposed constrained FGO approach leverages temporal correlations across multiple epochs, resulting in optimal state trajectory estimates while consistently maintaining constraint satisfaction. Experimental results confirm that our method outperforms conventional Kalman filter approaches, demonstrating its effectiveness and robustness for pedestrian navigation.

Figures (5)

• Figure 1: Architecture of the proposed FGO-based personal navigation algorithm. The subscript $1:T$ denotes the indices within the sliding window.
• Figure 2: The comparison between the ground truth (blue) and the estimated trajectory of FGO-ZUPT-POS-STEP (orange).
• Figure 3: The comparison between the ground truth (blue) and the estimated trajectory of EKF-ZUPT-POS-STEP (orange).
• Figure 4: Position measurements horizontal errors.
• Figure 5: The cumulative distribution function of horizontal position errors for EKF-based and FGO-based approaches for each foot.