Continuous latent state preintegration for inertial-aided systems

Traditionally, the pose and velocity prediction of a system at time t2 given inertial measurements from a 6-DoF IMU depends on the knowledge of the system's state at time t1. It involves a series of integration and double integration that can be computationally expensive if performed regularly, in particular in the context of inertial-aided optimisation-based state estimation. The concept of preintegration consists of creating pseudo-measurements that are independent of the system's initial conditions (pose and velocity at t1) in order to predict the system's state at t2. These pseudo-measurements, so-called preintegrated measurements, were originally computed numerically using the integration rectangle rule. This article presents a novel method to perform continuous preintegration using Gaussian processes (GPs) to model the system's dynamics focusing on high accuracy. It represents the preintegrated measurement's derivatives in a continuous latent state that is learnt/optimised according to asynchronous IMU gyroscope and accelerometer measurements. The GP models allow for analytical integration and double integration of the latent state to generate accurate preintegrated measurements called unified Gaussian preintegrated measurements (UGPMs). We show through extensive quantitative experiments that the proposed UGPMs outperform the standard preintegration method by an order of magnitude. Additionally, we demonstrate that the UGPMs can be integrated into off-the-shelf multi-modal estimation frameworks with ease based on lidar-inertial, RGBD-inertial, and visual-inertial real-world experiments.