Velocity-Guided Factor Graph Optimization for Foot-Mounted Inertial Odometry
VeloFGO is a neural-aided foot-mounted inertial odometry framework for pedestrian navigation in GNSS-denied environments. From raw foot-mounted accelerometer and gyroscope measurements, it estimates continuous three-dimensional (3-D) foot velocity in the IMU body frame. The predicted velocity is retained as an intermediate motion representation, and its magnitude determines the motion state used to activate physically defined stance constraints.
VeloFGO couples this learned motion cue with model-based inertial estimation. The inferred motion state governs navigation keyframe generation and the insertion of zero-velocity and zero-displacement factors. Together with IMU preintegration and bias-evolution factors, these constraints are jointly optimized in an incremental factor graph.
Raw IMU measurements are processed by two complementary branches and fused in the factor-graph backend:
- Learning-based motion inference: IMUVelocityNet predicts frame-wise 3-D body-frame velocity from normalized temporal IMU windows. The velocity magnitude provides a continuous cue for motion-state inference.
- Model-based inertial propagation: high-rate IMU measurements are summarized between navigation keyframes through IMU preintegration.
- Incremental factor-graph optimization: IMU, bias random-walk, zero-velocity, and zero-displacement factors are jointly optimized using iSAM2.
- Frame-wise 3-D foot-velocity regression from raw six-axis IMU measurements
- Velocity-guided motion-state inference and navigation keyframe generation
- Physically grounded zero-velocity and zero-displacement constraints
- Incremental factor-graph optimization with IMU preintegration
- Participant-disjoint evaluation across different walking profiles
- Cross-hardware evaluation using a low-cost BNO055 IMU without network retraining
On 20 participant-disjoint SPC2019 test sequences, VeloFGO achieves a mean absolute trajectory error (ATE) of 0.359 m, a 10-s relative trajectory error of 0.430 m, and a final drift of 0.369 m. This corresponds to a 44.3% reduction in mean ATE relative to SHOE-FGO. The output-representation ablation further supports 3-D velocity as the learned motion representation for the proposed estimator. Cross-hardware experiments use the same trained network weights with a low-cost BNO055 IMU.
Coming soon.
The source code, pretrained models, configuration files, data-preparation instructions, and evaluation tools are being prepared for public release.
The paper and BibTeX entry will be added upon publication.