Equivariant Filters for Visual Spatial Awareness

Abstract

Algorithms that estimate the navigation states and local environment of a robotic vehicle are core enabling technologies for autonomous systems. The first work that exploited symmetry and equivariance for state estimation in robotics was published in 2000-2005 and had immediate impact as an enabling technology for attitude estimation in the growth of the commercial aerial robotics industry. Recently, the discovery of a novel symmetry structure has allowed equivariant observer theory to be applied to Simultaneous Localisation and Mapping (SLAM), the archetypical spatial awareness problem in robotics. Beyond classical SLAM, however, it has remained unclear how to extend the equivariant observer theory to Visual Spatial Awareness (VSA); the general problem of using the rich visual information obtained from a camera, potentially in combination with other sensors, to model the physical relationship between a mobile robot and its environment. VSA will be one of the enabling technologies across a wide range of emerging autonomous systems applications including autonomous vehicles, aerial vehicles, virtual reality, and augmented reality. Essentially, any future autonomous system that operates in an unstructured dynamic environment alongside humans will need high-performance, robust VSA solutions. This thesis brings together the new field of equivariant systems theory with the application domain of visual spatial awareness, and contributes equally to the development of general theory motivated by the application domain and the development of real world algorithms based on the theory.

Existing equivariant observer designs suffer from key limitations. Constructive equivariant observers are specific to their application and difficult to generalise outside of the specific class of left- or right-invariant systems, while the invariant extended Kalman filter can only be applied to the subclass of group-affine systems defined with Lie group state space. To overcome these issues, this thesis proposes the Equivariant Filter (EqF), a general linearisation-based observer design that can be applied to any system exhibiting a Lie group symmetry. I show that it is possible to obtain a second-order (rather than first-order) approximation of the system measurement function when output is compatible with the chosen system symmetry. Observers based on this framework have demonstrably lower linearisation error, better performance, and better robustness than state-of-the-art stochastic filters.

To address the Visual SLAM (VSLAM) and Visual Inertial Odometry (VIO) problems, I develop new Lie groups, which are the first symmetries for which the measurements of VSLAM and VIO are equivariant. I use the new VSLAM symmetry to develop a novel constructive observer design for VSLAM with almost-globally asymptotically stabilisable error dynamics. For the VIO problem, I combine the newly proposed EqF with the new VIO symmetry and generate open-source code that exhibits better than state-of-the-art performance on standard robotics datasets and at a speed between two and six of comparable algorithms.

Through the introduction of the EqF and new symmetries for VSLAM and VIO, this thesis presents a clear advance in the theory of equivariant observers and a powerful new approach to filter design for VSA problems.