CAMSCKF: A Multi-State Constraint Kalman Filter with Adaptive Multivariate Noise Parameters Clustering and Estimation for Visual-Inertial Odometry

Abstract

The Visual-Inertial Odometry has been widely deployed on autonomous robots traveling in open outdoor scenarios. However, the visual measurements will be influenced heavily by the observation distances, perspectives, lighting and texture conditions, with distinct and time-varying noise distributions of measurements. Existing methods for handling time-varying noise in Visual-Inertial Odometry regard all measurement noise as identically distributed, unable to effectively deal with the distinct noise in open outdoor scenarios, which degrades the localization accuracy. In this paper, a Multi-State Constraint Kalman Filter with Adaptive multivariate noise parameters Clustering and estimation for visual-inertial odometry (CAMSCKF) is proposed to address the issue, which can separately track the measurement noise covariance matrix (MNCM) of different measurement clusters and adjust the MNCM in real-time. Firstly, the joint distribution of the state and the MNCM coefficients for each cluster is modeled as an Gaussian-Multivariate Generalized Inverse Gaussian distribution. Subsequently, an Expectation Maximization algorithm-based stepwise adaptive measurement clustering method is designed, which clusters measurements according to their corresponding innovations. Finally, an analytical update method for the joint posterior distribution without fixed-point iteration is implemented, achieving adaptive adjustment of the MNCM, thereby enabling accurate and robust Visual-Inertial Odometry localization. The superiority of the proposed method is demonstrated by simulations and dataset experiments, especially under the aggressive motion. In the experiments of the challenging outdoor dataset UZH-FPV, the proposed method has improved the average position and attitude estimation accuracy by 35.69% and 32.88%, respectively, compared with the state-of-the-art ANGIG-KF.