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

In this article, we develop and open-source, for the first time, a robust and efficient square-root filter (SRF)-based visual–inertial navigation system (VINS), termed $\sqrt{\text{VINS}}$, 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 semidefiniteness. 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 and the SRF to preserve the upper triangular structure of square-root covariance. Moreover, we design a fast, robust, and dynamic initialization method, which first quickly recovers the minimal states without triangulating 3D features and then efficiently performs iterative SRF update to refine the full states, enabling seamless VINS operation even in challenging scenarios. The proposed LLT-based SRF is extensively verified through numerical studies, demonstrating superior numerical stability under challenging conditions and achieving robust efficient performance on 32-b single-precision floats, operating at twice the speed of state-of-the-art 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 $\sqrt{\text{VINS}}$ is extensively validated across diverse scenarios, demonstrating strong efficiency, robustness, and reliability.