PC-SRIF: Preconditioned Cholesky-based Square Root Information Filter for Vision-aided Inertial Navigation

TL;DR

This paper introduces PC-SRIF, a new preconditioned Cholesky-based filter for vision-aided inertial navigation that offers improved efficiency and numerical stability over traditional QR-based methods, especially in single precision computations.

Contribution

The paper proposes a novel preconditioning technique for Cholesky-based Square Root Information Filters, enabling stable and faster VINS computations in single precision.

Findings

PC-SRIF is 41% faster than QR-based SRIF in experiments.

PC-SRIF maintains numerical stability with Cholesky decomposition in single precision.

Theoretical analysis shows improved conditioning leads to efficiency gains.

Abstract

In this paper, we introduce a novel estimator for vision-aided inertial navigation systems (VINS), the Preconditioned Cholesky-based Square Root Information Filter (PC-SRIF). When solving linear systems, employing Cholesky decomposition offers superior efficiency but can compromise numerical stability. Due to this, existing VINS utilizing (Square Root) Information Filters often opt for QR decomposition on platforms where single precision is preferred, avoiding the numerical challenges associated with Cholesky decomposition. While these issues are often attributed to the ill-conditioned information matrix in VINS, our analysis reveals that this is not an inherent property of VINS but rather a consequence of specific parameterizations. We identify several factors that contribute to an ill-conditioned information matrix and propose a preconditioning technique to mitigate these conditioning issues. Building on this analysis, we present PC-SRIF, which exhibits remarkable stability in performing Cholesky decomposition in single precision when solving linear systems in VINS. Consequently, PC-SRIF achieves superior theoretical efficiency compared to alternative estimators. To validate the efficiency advantages and numerical stability of PC-SRIF based VINS, we have conducted well controlled experiments, which provide empirical evidence in support of our theoretical findings. Remarkably, in our VINS implementation, PC-SRIF's runtime is 41% faster than QR-based SRIF.