sqrtVINS: Robust and Ultrafast Square-Root Filter-based 3D Motion Tracking

2025-10-11

AI Summary

To address critical challenges in embedded visual-inertial navigation systems (VINS) under resource constraints and numerical instability—particularly during dynamic initialization with ultra-short time windows (e.g., 100 ms)—this paper proposes and open-sources sqrtVINS, the first lightweight VINS based on square-root filtering. Methodologically, it introduces: (1) a square-root filter update mechanism preserving the triangular structure of Cholesky factors, significantly enhancing numerical stability and computational efficiency in 32-bit single-precision arithmetic; and (2) a dynamic state initialization strategy that eliminates the need for 3D feature triangulation, markedly improving initialization success rate within 100-ms windows. Built upon tightly coupled fusion, LLT decomposition, and iterative optimization, sqrtVINS achieves high accuracy while enabling ultra-fast execution—demonstrating twice the runtime speed of state-of-the-art methods—and supports real-time 3D motion tracking on mobile devices.

Technology Category

• Intelligent Robots: State Estimation
• Computer Vision: Motion & Tracking
• Planning, Routing, and Scheduling: Optimization of Spatio-temporal Systems

Application Category

• Systems and Infrastructure for Web, Mobile and WoT: Virtualization and resource management in Web systems and infrastructures
• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• Security and Privacy: Large-scale security measurements

Abstract

In this paper, we develop and open-source, for the first time, a square-root filter (SRF)-based visual-inertial navigation system (VINS), termed sqrtVINS, which is ultra-fast, numerically stable, and capable of dynamic initialization even under extreme conditions (i.e., extremely small time window). Despite recent advancements in VINS, resource constraints and numerical instability on embedded (robotic) systems with limited precision remain critical challenges. A square-root covariance-based filter offers a promising solution by providing numerical stability, efficient memory usage, and guaranteed positive semi-definiteness. However, canonical SRFs suffer from inefficiencies caused by disruptions in the triangular structure of the covariance matrix during updates. The proposed method significantly improves VINS efficiency with a novel Cholesky decomposition (LLT)-based SRF update, by fully exploiting the system structure to preserve the structure. Moreover, we design a fast, robust, dynamic initialization method, which first recovers the minimal states without triangulating 3D features and then efficiently performs iterative SRF update to refine the full states, enabling seamless VINS operation. The proposed LLT-based SRF is extensively verified through numerical studies, demonstrating superior numerical stability and achieving robust efficient performance on 32-bit single-precision floats, operating at twice the speed of state-of-the-art (SOTA) methods. Our initialization method, tested on both mobile workstations and Jetson Nano computers, achieving a high success rate of initialization even within a 100 ms window under minimal conditions. Finally, the proposed sqrtVINS is extensively validated across diverse scenarios, demonstrating strong efficiency, robustness, and reliability. The full open-source implementation is released to support future research and applications.

Problem

• Enhancing numerical stability for embedded robotic systems
• Achieving ultrafast VINS performance with efficient memory usage
• Enabling robust dynamic initialization under extreme conditions

Innovation

• Cholesky decomposition-based square-root filter update
• Dynamic initialization without 3D feature triangulation
• Iterative SRF refinement for full state optimization