Constrained Factor Graph Optimization for Robust Networked Pedestrian Inertial Navigation

2025-05-13

AI Summary

This work addresses severe IMU integration drift in networked pedestrian inertial navigation. We propose a novel nonlinear factor graph optimization framework incorporating kinematic constraints. Specifically, we jointly model zero-velocity updates (ZUPT) as equality constraints and anatomy-inspired inter-joint distance inequality constraints. Crucially, we introduce a differentiable Softmax penalty term to smoothly embed hard inequality constraints into the factor graph—first such formulation in the literature. Furthermore, we achieve, for the first time within the FGo framework, unified modeling of spatiotemporal correlations across multi-body IMUs and biomechanical constraints. Evaluated on real-world walking sequences, our method reduces trajectory error by 42% compared to conventional Kalman filtering, while strictly satisfying anatomical distance constraints throughout the entire trajectory. This yields substantial improvements in both estimation accuracy and robustness.

Technology Category

• Intelligent Robots: State Estimation
• Constraint Satisfaction and Optimization: Constraint Optimization
• Planning, Routing, and Scheduling: Optimization of Spatio-temporal Systems

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• User Modeling, Personalization and Recommendation: Federated recommendation systems and personalization
• Semantics and Knowledge: Methods to enhance, augment, integrate or synergize semantic models such as knowledge graphs and LLMs

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.

Problem

• Mitigate drift in pedestrian inertial navigation using FGO
• Integrate kinematic constraints into nonlinear optimization framework
• Enforce inequality constraints via softmax penalty in FGO

Innovation

• Constrained FGO integrates kinematic constraints directly
• Softmax penalty enforces inequality constraints robustly
• Leverages temporal correlations for optimal trajectory estimates