Equivariant Filter Design for Inertial Navigation Systems with Input Measurement Biases

Abstract

This letter re-visits the problem of visual-inertial navigation system (VINS) and presents a novel filter design we dub the multi state constraint equivariant filter (MSCEqF, in analogy to the well known MSCKF). We define a symmetry group and corresponding group action that allow specifically the design of an equivariant filter for the problem of visualinertial odometry (VIO) including IMU bias, and camera intrinsic and extrinsic calibration states. In contrast to state-of-the-art invariant extended Kalman filter (IEKF) approaches that simply tack IMU bias and other states onto the SE2(3) group, our filter builds upon a symmetry that properly includes all the states in the group structure. Thus, we achieve improved behavior, particularly when linearization points largely deviate from the truth (i.e., on transients upon state disturbances). Our approach is inherently consistent even during convergence phases from significant errors without the need for error uncertainty adaptation, observability constraint, or other consistency enforcing techniques. This leads to greatly improved estimator behavior for significant error and unexpected state changes during, e.g., longduration missions. We evaluate our approach with a multitude of different experiments using three different prominent real-world datasets.

Geometric stochastic filter with guaranteed performance for autonomous navigation based on IMU and feature sensor fusion

“…To this end, the true navigation kinematics of a rigid-body traveling with 6 DoF are as follows [2], [5], [6]:…”

“…Common causes of GPS signal loss are multipath, obstructions, fading, and denial in indoor environments which create the need for a backup navigation solution. In the recent years, a number of GPS-denied navigation solutions have been developed, for instance, vision-aided-based navigation [2], [3], [5], [6] (monocular or stereo camera) and Light Detection and Ranging (LiDAR)-based navigation or 3D laser scanners [1]. However, rapid advances in the areas of Microelectromechanical systems (MEMS) and communication technology motivates the development of navigation solutions reliant on the fusion of Ultra-wideband (UWB) and Inertial Measurement Unit (IMU) sensors due to their reduced price and weight, and compactness in contrast with other aided navigation units.…”

MSCEqF: A Multi State Constraint Equivariant Filter for Vision-Aided Inertial Navigation

“…In very recent research, van Goor et al introduced the EqF [14,15] as a general filter design for systems on homogeneous spaces, and proposed a symmetry for fixed landmark measurements in the context of VI-SLAM [16,17,18,19,20]. Later, Fornasier et al proposed a novel symmetry for INS that couples navigation states and IMU bias and developed an EqF design for INS [21,22] that proved superior to state-of-the-art in terms of robustness to wrong initialization, transient behavior, and consistency properties. In a very recent research study [23], the same authors analyzed the theoretical properties of different symmetry groups when employed in designing filters for inertial navigation systems, and provided a discussion of the relative strengths and weaknesses of different filter algorithms.…”

Overcoming Bias: Equivariant Filter Design for Biased Attitude Estimation With Online Calibration

“…This symmetry draws from advances in the field of equivariant system theory and observer design [9], [10], [8], [11] and provides a framework for stochastic filter design that applies to a broader class of systems than the IEKF while specialising back to the IEKF for groupaffine systems on Lie-groups [12]. In [13], a new symmetry was introduced to couple the navigation state as well as the accelerometer and gyroscope biases in a single geometric structure. This was only possible by extending the system state with an artificial velocity bias state.…”

“…(i): We propose a new integrated equivariant symmetry; a group structure of (SO (3) ⋉ so (3)) × SO(3) n with n ≤ N calibration states for the generalized attitude estimation problem (inspired from [13]) that includes biases in a minimal state vector and which is modularly extensible to N sensors with their calibration states measuring both body-frame or spatial reference directions.…”