Visual-inertial odometry (VIO), which fuses noisy inertial readings and camera measurements to provide 3D motion tracking, is a foundational component in many autonomous applications. With the increasing use of next-generation edge devices (e.g., AR/VR devices, nano drones, and mobile robotics) that are constrained by limited power, resources, and multitasking demands, balancing computational efficiency and accuracy in VIO estimators has become more critical than ever. Historically, state estimation algorithms have been developed using either optimization or filtering-based methods, with the key distinction being the ability to relinearize measurements and correct state estimates iteratively. It has been widely claimed that iterative methods improve accuracy by allowing for the reduction of error through relinearization at a higher computational demand. Conversely, filtering methods are more efficient but may suffer from significant linearization errors. However, these trade-offs have not been thoroughly examined in the context of visual-inertial motion tracking. In this paper, we conduct the first comprehensive study on the impact of iterative algorithms in sliding-window VIO. We analyze the relinearization of IMU and camera measurements separately, providing insights into how each affects system performance. By considering key factors such as system observability and measurement processes, we offer a deeper understanding of VIO estimator behavior. Our findings, backed by real-world tests, offer practical guidelines for balancing accuracy and efficiency, helping practitioners determine when to prioritize iterative methods or simpler filtering approaches while encouraging researchers and engineers to rethink VIO design for optimal resource allocation.

Energy-efficient visual-inertial motion tracking on SWAP-constrained edge devices (e.g., drones and AR glasses) is essential but challenging. Our previous work [1] introduced the first-of-its-kind quantized visual-inertial odometry (QVIO), utilizing either raw measurement quantization (zQVIO) or single-bit residual quantization (rQVIO). While QVIO has demonstrated significant data transfer reduction with competitive performance, it has limitations. Specifically, zQVIO directly quantizes raw measurements into multi-bit values, while requiring the ad-hoc inflation of measurement noise to account for quantization errors. On the other hand, rQVIO is limited to single-bit measurement with certain accuracy loss. This work introduces QVIO2 to address these issues. The proposed QVIO2 improves data quantization strategies and derives a Maximum A Posteriori (MAP) quantized estimator that rigorously handles both multi-bit and single-bit, raw and residual quantized measurements in a unified manner. These improvements lead to more communication-efficient and accurate systems. Additionally, we optimize the communication protocol to further reduce data transfer by eliminating unnecessary transmissions. Extensive numerical and experimental results demonstrate reduced communication requirements and improved accuracy. Compared to the previous QVIO system, zQVIO2 achieves the same accuracy with a 30 % reduction in data transfer, while rQVIO2 improves accuracy without increasing data communication. In real-world scenarios, our new zQVIO2 and rQVIO2 have demonstrated nearly no accuracy loss with only 4.6 bits and 3.5 bits of data communication, achieving compression rates of$7 \times$and$9.1 \times$.

The system observability analysis is of practical importance, for example, due to its ability to identify the unobservable directions of the estimated state which can influence estimation accuracy and help develop consistent and robust estimators. Recent studies focused on analyzing the observability of the state of various multisensor systems with a particular interest in unobservable directions induced by degenerate motions. However, those studies mostly stay in the specific sensor domain without aiding to extend the understanding to other heterogeneous systems. To this end, in this work, we provide degenerate motion analysis on general local and global sensor-paired systems, offering insights applicable to a wide range of existing navigation systems. Our analysis includes 9 degenerate motion identification including 5 already identified in literature and 4 new motions with both synchronous and asynchronous sensor-pair cases. Comprehensive numerical studies are conducted to verify those identified motions, show the effect of degenerate motion on state estimation, and demonstrate the generalizability of our analysis on various multisensor systems.

In this paper, we introduce a novel and efficient technique for consistent covariance recovery in nonlinear optimization-based Visual-Inertial Navigation Systems (VINS). Estimating uncertainty in real-time is crucial for evaluating system performance and enhancing downstream operations such as data association. However accessing the marginal covariance of the state variables of interest in optimization-based VINS presents a significant challenge – a computational bottleneck due to the need to invert the high-dimensional information (Hessian) matrix. In our recent work [1], the First-Estimates Jacobian (FEJ) methodology was used to properly fix state linearization points in the optimization-based VINS, which seems counter-intuitive but improves the estimation performance in both consistency and accuracy. Capitalizing on this unique aspect of the FEJ strategy, in this work we carefully design the covariance recovery algorithm to improve efficiency by avoiding redundant computation. Remarkably, our approach achieves a computational speed that is 4-10 times faster than the existing methods. Through comprehensive numerical evaluations across four state-of-the-art marginalization archetypes, we not only affirm the consistency of our covariance estimates but underscore its superior computational efficiency.

Achieving efficient and consistent localization with a prior map remains challenging in robotics. Conventional keyframe-based approaches often suffer from sub-optimal viewpoints due to limited field of view (FOV) and/or constrained motion, thus degrading the localization performance. To address this issue, we design a real-time tightly-coupled Neural Radiance Fields (NeRF)-aided visual-inertial navigation system (VINS). In particular, by effectively leveraging the NeRF’s potential to synthesize novel views, the proposed NeRF-VINS overcomes the limitations of traditional keyframe-based maps (with limited views) and optimally fuses IMU, monocular images, and synthetically rendered images within an efficient filter-based framework. This tightly-coupled fusion enables efficient 3D motion tracking with bounded errors. We extensively validate the proposed NeRF-VINS against the state-of-the-art methods that use prior map information, and demonstrate its ability to perform real-time localization, at 15 Hz, on a resource-constrained Jetson AGX Orin embedded platform.

In this paper, we strongly advocate square-root covariance (instead of information) filtering for Visual-Inertial Navigation Systems (VINS), in particular on resource-constrained edge devices, because of its superior efficiency and numerical stability. Although VINS have made tremendous progress in recent years, they still face resource stringency and numerical instability on embedded systems when imposing limited word length. To overcome these challenges, we develop an ultrafast and numerically-stable square-root filter (SRF)-based VINS algorithm (i.e., SR-VINS). The numerical stability of the proposed SR-VINS is inherited from the adoption of square-root covariance while the remarkable efficiency is largely enabled by the novel SRF update method that is based on our new permuted-QR (P-QR), which fully utilizes and properly maintains the upper triangular structure of the square-root covariance matrix. Furthermore, we choose a special ordering of the state variables which is amenable for (P-)QR operations in the SRF propagation and update and prevents unnecessary computation. The proposed SR-VINS is validated extensively through numerical studies, demonstrating that when the state-of-the-art (SOTA) filters have numerical difficulties, our SR-VINS has superior numerical stability, and remarkably, achieves efficient and robust performance on 32-bit single-precision float at a speed nearly twice as fast as the SOTA methods. We also conduct comprehensive real-world experiments to validate the efficiency, accuracy, and robustness of the proposed SR-VINS.

As edge devices equipped with cameras and inertial measurement units (IMUs) are emerging, it holds huge implications to endow these mobile devices with spatial computing capability. However, ultra-efficient visual-inertial estimation at the size, weight and power (SWAP)-constrained edge devices to provide accurate 3D motion tracking remains challenging. This is exacerbated by data transfer (between different processors and memory) that consumes significantly more energy than computing itself. To push the state of the art, this paper proposes the first-of-its-kind quantized visual-inertial odometry (QVIO) to offer energy-efficient 3D motion tracking. In particular, we first quantize raw visual measurements in an intuitive way with a given small number of bits and then perform an EKF update with these quantized measurements (termed zQVIO). To improve this ad-hoc quantizer (although it works well in practice), we systematically quantize each measurement residual into a single bit and perform maximum-a-posterior (MAP) estimation. measurements. Thanks to these quantizers, the proposed QVIO estimators significantly reduce the data transfer and thus improve energy efficiency. As shown in our extensive experiments, the proposed residual-quantized VIO (rQVIO) achieves remarkably competing performance even when using an average of only 3.7 bits per measurement, equivalent to a data reduction of 8.6 times compared to transmitting single-precision measurements.

State-of-the-art monocular visual-inertial odometry (VIO) approaches rely on sparse point features in part due to their efficiency, robustness, and prevalence, while ignoring high-level structural regularities such as planes that are common to man-made environments and can be exploited to further constrain motion. Generally, planes can be observed by a camera for significant periods of time due to their large spatial presence and thus, are amenable for long-term navigation. Therefore, in this paper, we design a novel real-time monocular VIO system that is fully regularized by planar features within a lightweight multi-state constraint Kalman filter (MSCKF). At the core of our method is an efficient robust monocular-based plane detection algorithm, which does not require additional sensing modalities such as a stereo or depth camera as commonly seen in the literature, while enabling real-time regularization of point features to environmental planes. Specifically, in the proposed MSCKF, long-lived planes are maintained in the state vector, while shorter ones are marginalized after use for efficiency. Planar regularities are applied to both in-state SLAM features and out-of-state MSCKF features, thus fully exploiting the environmental plane information to improve VIO performance. The proposed approach is evaluated with extensive Monte-Carlo simulations and different real-world experiments including an author-collected AR scenario, and shown to outperform the point-based VIO in structured environments. Video Demonstration https://youtu.be/bec7LbYaOS8AR Table Dataset https://github.com/rpng/ar_table_dataset

In this work, we present a comprehensive analysis of the application of the First-estimates Jacobian (FEJ) design methodology in nonlinear optimization-based Visual-Inertial Navigation Systems (VINS). The FEJ approach fixes system linearization points to preserve proper observability properties of VINS and has been shown to significantly improve the estimation performance of state-of-the-art filtering-based methods. However, its direct application to optimization-based estimators holds challenges and pitfalls, which we addressed in this paper. Specifically, we carefully examine the observability and its relation to inconsistency and FEJ, based on this, we explain how to properly apply and implement FEJ within four marginalization archetypes commonly used in non-linear optimizationbased frameworks. FEJ's effectiveness and applications to VINS are investigated and demonstrate significant performance improvements. Additionally, we offer a detailed discussion of results and guidelines on how to properly implement FEJ in optimization-based estimators.

In this paper, we propose a novel consistent state estimator design for visual-inertial systems. Motivated by first-estimates Jacobian (FEJ) based estimators - which uses the first-ever estimates as linearization points to preserve proper observability properties of the linearized estimator thereby improving the consistency - we carefully model measurement linearization errors due to its Jacobian evaluation and propose a methodology which still leverages FEJ to ensure the estimator's observability properties, but additionally explicitly compensate for linearization errors caused by poor first estimates. We term this estimator FEJ2, which directly addresses the discrepancy between the best Jacobian evaluated at the latest state estimate and the first-estimates Jacobian evaluated at the first-time-ever state estimate. We show that this process explicitly models that the FEJ used is imperfect and thus contributes additional error which, as in FEJ2, should be modeled and consistently increase the state covariance during update. The proposed FEJ2 is evaluated against state-of-the-art visual-inertial estimators in both Monte-Carlo simulations and real-world experiments, which has been shown to outperform existing methods and to robustly handle poor first estimates and high measurement noises.

System modeling and parameter identification of micro aerial vehicles (MAV) are crucial for robust autonomy, especially under highly dynamic motions. Visual-inertial-aided online parameter identification has recently seen research attention due to the demanding of adaptation to platform configuration changes with minimal onboard sensor requirements. To this end, we design an online MAV system identification algorithm to tightly fuse visual, inertial and MAV aerodynamic information within a lightweight multi-state constraint Kalman filter (MSCKF) framework. In particular, while one could blindly fuse the MAV dynamic-induced relative motion constraints in EKF, we numerically show that due to the (quadrotor) MAV system modeling inaccuracy, they often become overconfident and negatively impact the state estimates. As such, we leverage the Schmidt-Kalman filter (SKF) for MAV system parameter identification to prevent corruption of state estimates. Through extensive simulations and real-world experiments, we validate the proposed SKF-based scheme and demonstrate its ability to perform robust system identification even in the presence of an inconsistent MAV dynamic model under different motions.

Batch optimization based inertial measurement unit (IMU) and visual sensor fusion enables high rate localization for many robotic tasks. However, it remains a challenge to ensure that the batch optimization is computationally efficient while being consistent for high rate IMU measurements without marginalization. In this paper, we derive inspiration from maximum likelihood estimation with partial-fixed estimates to provide a unified approach for handing both IMU preintegration and time-offset calibration. We present a modularized analytic combined IMU integrator (ACI2) with elegant derivations for IMU integrations, bias Jabcobians and related covariances. To simplify our derivation, we also prove that the right Jacobians for Hamilton quaterions and SO(3) are equivalent. Finally, we present a time offset calibrator that operates by fixing the linearization point for a given time offset. This reduces re-integration of the IMU measurements and thus improve efficiency. The proposed ACI2and time-offset calibration is verified by intensive Monte-Carlo simulations generated from real world datasets. A proof-of-concept real world experiment is also conducted to verify the proposed ACI2estimator.

Aiming for a lightweight and robust localization solution for low-cost, low-power autonomous robot platforms, such as educational or industrial ground vehicles, under challenging conditions (e.g., poor sensor calibration, low lighting and dynamic objects), we propose a two-stage localization system which incorporates both offline prior map building and online multi-modal localization. In particular, we develop an occupancy grid mapping system with probabilistic odometry fusion, accurate scan-to-submap covariance modeling, and accelerated loop-closure detection, which is further aided by 2D line features that exploit the environmental structural constraints. We then develop a versatile EKF-based online localization system which optimally (up to linearization) fuses multi-modal information provided by the pre-built occupancy grid map, IMU, odometry, and 2D LiDAR measurements with low computational requirements. Importantly, spatiotemporal calibration between these sensors are also estimated online to account for poor initial calibration and make the system more "plug-and-play", which improves both the accuracy and flexibility of the proposed multi-sensor fusion framework. In our experiments, our mapping system is shown to be more accurate than the state-of-the-art Google Cartographer. Then, extensive Monte-Carlo simulations are performed to verify both accuracy, consistency and efficiency of the proposed map-based localization system with full spatiotemporal calibration. We also validate the complete system (prior map building and online localization) with building-scale real-world datasets.