The extended Kalman filter (EKF) has been the industry standard for state estimation problems over the past sixty years. The Invariant Extended Kalman Filter (IEKF) is a recent development of the EKF for the class of group-affine systems on Lie groups that has shown superior performance for inertial navigation problems. The IEKF comes in two versions, left- and right- handed respectively, and there is a perception in the robotics community that these filters are different and one should choose the handedness of the IEKF to match handedness of the measurement model for a given filtering problem. In this paper, we revisit these algorithms and demonstrate that the left- and right- IEKF algorithms (with reset step) are identical, that is, the choice of the handedness does not affect the IEKF's performance when the reset step is properly implemented. The reset step was not originally proposed as part of the IEKF, however, we provide simulations to show that the reset step improves asymptotic performance of all versions of the the filter, and should be included in all high performance algorithms. The GNSS-aided inertial navigation system (INS) is used as a motivating example to demonstrate the equivalence of the two filters.

Image tracking is the problem of estimating the transformation that relates a moving image of a scene to an original reference image. The problem is important in control of autonomous vehicles or robots, where the image encodes information about the motion of the camera or environment, as well as in pure computer vision applications. In this paper, we present an equivariant filter design for high performance tracking of planar image transformations using an event camera. The design exploits the Asynchronous Event Blob (AEB) tracker (Wang et al., 2024) to extract feature-position measurements from the raw event stream, and an equivariant filter to compute an affine image translation and rotation using the special Euclidean group symmetry. The equivariant filter incorporates an equivalent-measurement update step that de-correlates the (highly temporally correlated) feature-position measurements provided by the AEB tracker. We evaluate the design experimentally using two datasets involving general and fast rotational motion. We benchmark results against direct optimisation (estimating the relative transformation from the raw blob tracks), and a covariance intersection approach for overcoming data correlation. Our design provides smooth image tracking for features moving up to 7000 pixels per second on the image plane.

This paper addresses the problem of homography estimation using a nonlinear observer designed on the Lie group 𝐒𝐋(3) that exploits the full image information through direct image registration. Unlike traditional feature-based methods, which rely on extensive feature extraction and matching, the proposed approach formulates an observer that minimises a cost function defined directly in terms of image pixel intensities. Explicit conditions ensuring the non-degeneracy of the cost function are derived, and a comprehensive analysis is conducted to characterise and generate degenerate (unobservable) image configurations. Theoretical results demonstrate local exponential convergence of the observer. To improve local convergence properties, a second-order observer variant is introduced by incorporating the Hessian of the cost function into the correction term. Simulation results demonstrate the performance of the proposed solutions on real images.

Many Inertial Navigation Systems (INS) use Global Navigation Satellite System (GNSS) position as the primary measurement to drive filter performance and bound error growth. However, commercial-grade GNSS receivers introduce unknown measurement delays ranging from 50 ms to 300 ms depending on sensor quality and operating mode. Such time delays can significantly degrade INS performance unless they are explicitly compensated for. Existing algorithms commonly estimate this delay offline, run the filter concurrently with GNSS measurements using buffered Inertial Measurement Unit (IMU) data, and predict the current state by forward-integrating buffered inertial measurements via IMU preintegration. The state-of-the-art online method is an Extended Kalman Filter (EKF) that explicitly models the time delay as a state parameter, which defines the preintegration duration. This paper introduces a novel geometric framework for modeling time-delayed INS, in which Galilean symmetry is leveraged to provide a joint representation of space and time for consistent state estimation. An Equivariant Filter (EqF) is derived for the coupled estimation of navigation states and time delay. Validation is performed on two fixed-wing Uncrewed Aerial Vehicles (UAV) with GNSS time lags of 90 ms and 120 ms. The test flights last two to three minutes. Simulations further investigate delays up to 500 ms and provide a statistical comparison against the state-of-the-art EKF. Results show that the EqF preserves accuracy and consistency, while the EKF lacks consistency and its performance degrades significantly with increasing measurement delays.

This paper introduces the concept of a synchronous model as an extension of the internal model concept used in observer design for dynamical systems. A system is said to contain a synchronous model of another if there is a suitable error function between the two systems that remains stationary for all of the trajectories of the two systems. A system is said to admit a synchronous lift if a second system containing a synchronous model exists. We provide necessary and sufficient conditions that a system admits a synchronous lift and provide a method to construct a (there may be many) lifted system should one exist. We characterise the class of all systems that admit a synchronous lift by showing that they consist of fundamental vector fields induced by a Lie group action, a class of system we term fundamental systems. For fundamental systems we propose a simple synchronous observer design methodology, for which we show how correction terms can be discretised and combined easily, facilitating global characterisation of convergence and performance. Finally, we provide three examples to demonstrate the key concepts of synchrony, symmetry construction, and observer design for a fundamental system.

Underwater vehicles are naturally modelled as rigid bodies on SE(3) subjected to added mass effects. The passivity of the Hamiltonian structure of the system can be exploited to design energy-based stabilising controllers, however, the extension of these control designs to tracking control is not trivial since the error system for the classical error formulations is not itself Hamiltonian. In this paper, we show that a novel choice of error function leads to error dynamics that are Hamiltonian. We go on to derive an energy-based tracking control for a fully coupled model of a submersible vehicle. Asymptotic convergence of the control scheme is proved and the control is demonstrated in a simulation study of the Blue Robotics BlueROV2 Heavy submersible.

Events cameras are ideal sensors for enabling robots to detect and track objects in highly dynamic environments due to their low latency output, high temporal resolution, and high dynamic range. In this paper, we present the Asynchronous Event Multi-Object Tracking (AEMOT) algorithm for detecting and tracking multiple objects by processing individual raw events asynchronously. AEMOT detects salient event blob features by identifying regions of consistent optical flow using a novel Field of Active Flow Directions built from the Surface of Active Events. Detected features are tracked as candidate objects using the recently proposed Asynchronous Event Blob (AEB) tracker in order to construct small intensity patches of each candidate object. A novel learnt validation stage promotes or discards candidate objects based on classification of their intensity patches, with promoted objects having their position, velocity, size, and orientation estimated at their event rate. We evaluate AEMOT on a new Bee Swarm Dataset, where it tracks dozens of small bees with precision and recall performance exceeding that of alternative event-based detection and tracking algorithms by over 37%. Source code and the labelled event Bee Swarm Dataset will be open sourced

As satellites become smaller, the ability to maintain stable pointing decreases as external forces acting on the satellite come into play. At the same time, reaction wheels used in the attitude determination and control system (ADCS) introduce high frequency jitter which can disrupt pointing stability. For space domain awareness (SDA) tasks that track objects tens of thousands of kilometres away, the pointing accuracy offered by current nanosats, typically in the range of 10 to 100 arcseconds, is not sufficient. In this work, we develop a novel payload that utilises a neuromorphic event sensor (for high frequency and highly accurate relative attitude estimation) paired in a closed loop with a piezoelectric stage (for active attitude corrections) to provide highly stable sensor-specific pointing. Event sensors are especially suited for space applications due to their desirable characteristics of low power consumption, asynchronous operation, and high dynamic range. We use the event sensor to first estimate a reference background star field from which instantaneous relative attitude is estimated at high frequency. The piezoelectric stage works in a closed control loop with the event sensor to perform attitude corrections based on the discrepancy between the current and desired attitude. Results in a controlled setting show that we can achieve a pointing accuracy in the range of 1-5 arcseconds using our novel payload at an operating frequency of up to 50Hz using a prototype built from commercial-off-the-shelf components. Further details can be found at https://ylatif.github.io/ultrafinestabilisation

This paper tackles the problem of estimating the relative position, orientation, and velocity between a UAV and a planar platform undergoing arbitrary 3D motion during approach and landing. The estimation relies on measurements from Inertial Measurement Units (IMUs) mounted on both systems, assuming there is a suitable communication channel to exchange data, together with visual information provided by an onboard monocular camera, from which the bearing (line-of-sight direction) to the platform's center and the normal vector of its planar surface are extracted. We propose a cascade observer with a complementary filter on SO(3) to reconstruct the relative attitude, followed by a linear Riccati observer for relative position and velocity estimation. Convergence of both observers is established under persistently exciting conditions, and the cascade is shown to be almost globally asymptotically and locally exponentially stable. We further extend the design to the case where the platform's rotation is restricted to its normal axis and show that its measured linear acceleration can be exploited to recover the remaining unobservable rotation angle. A sufficient condition to ensure local exponential convergence in this setting is provided. The performance of the proposed observers is validated through extensive simulations.

The extended Kalman filter (EKF) has been the industry standard for state estimation problems over the past sixty years. The classical formulation of the EKF is posed for nonlinear systems defined on global Euclidean spaces. The design methodology is regularly applied to systems on smooth manifolds by choosing local coordinates, however, it is well known that this approach is not intrinsic to the manifold and performance depends heavily on choosing 'good' coordinates. In this paper, we propose an extended Kalman filter that is adapted to the specific geometry of the manifold in question. We show that an affine connection and the concepts of parallel transport, torsion, and curvature are the key geometric structures that allow the formulation of a suitable family of intrinsic Gaussian-like distributions and provide the tools to understand how to propagate state estimates and fuse measurements. This leads us to propose novel geometric modifications to the propagation and update steps of the EKF and revisit recent work on the geometry of the reset step. The relative performance of the proposed geometric modifications are benchmarked against classical EKF and iterated EKF algorithms on a simplified inertial navigation system with direct pose measurements and no bias.

Mechanical control systems such as aerial, marine, space, and terrestrial robots often naturally admit a state-space that has the structure of a Lie group. The kinetic energy of such systems is commonly invariant to the induced action by the Lie group, and the system dynamics can be written as a coupled ordinary differential equation on the group and the dual space of its Lie algebra, termed a Lie-Poisson system. In this paper, we show that Lie-Poisson systems can also be written as a left-invariant system on a semi-direct Lie group structure placed on the trivialised cotangent bundle of the symmetry group. The authors do not know of a prior reference for this observation and we are confident the insight has never been exploited in the context of tracking control. We use this representation to build a right-invariant tracking error for the full state of a Lie-Poisson mechanical system and show that the error dynamics for this error are themselves of Lie-Poisson structure, albeit with time-varying inertia. This allows us to tackle the general trajectory tracking problem using an energy shaping design metholodology. To demonstrate the approach, we apply the proposed design methodology to a simple attitude tracking control.

An Inertial Navigation System (INS) is a system that integrates acceleration and angular velocity readings from an Inertial Measurement Unit (IMU), along with other sensors such as GNSS position, GNSS velocity, and magnetometer, to estimate the attitude, velocity, and position of a vehicle. This paper shows that the INS problem can be analysed using the automorphism group of the extended special Euclidean group: a group we term the extended similarity group. By exploiting this novel geometric framework, we propose an observer architecture with synchronous error dynamics; that is, the error is stationary if the observer correction terms are set to zero. In turn, this enables us to derive a modular, or plug-and-play, observer design for INS that allows different sensors to be added or removed depending on what is available in the vehicle sensor suite. We prove both almost-global asymptotic and local exponential stability of the error dynamics for the common scenario of at least IMU and GNSS position. To the authors' knowledge, this is the first non-linear observer design with almost global convergence guarantees or with plug-and-play modular capability. A simulation with extreme initial error demonstrates the almost-global robustness of the system. Real-world capability is demonstrated on data from a fixed-wing UAV, and the solution is compared to the state-of-the-art ArduPilot INS.

Range-only Simultaneous Localisation and Mapping (RO-SLAM) is of interest due to its practical applications in ultra-wideband (UWB) and Bluetooth Low Energy (BLE) localisation in terrestrial and aerial applications and acoustic beacon localisation in submarine applications. In this work, we consider a mobile robot equipped with an inertial measurement unit (IMU) and a range sensor that measures distances to a collection of fixed landmarks. We derive an equivariant filter (EqF) for the RO-SLAM problem based on a symmetry Lie group that is compatible with the range measurements. The proposed filter does not require bootstrapping or initialisation of landmark positions, and demonstrates robustness to the no-prior situation. The filter is demonstrated on a real-world dataset, and it is shown to significantly outperform a state-of-the-art EKF alternative in terms of both accuracy and robustness.

Koopman-based methods leverage a nonlinear lifting to enable linear regression techniques. Consequently, data generation, learning and prediction is performed through the lens of this lifting, giving rise to a nonlinear manifold that is invariant under the Koopman operator. In data-driven approximation such as Extended Dynamic Mode Decomposition, this invariance is typically lost due to the presence of (finite-data) approximation errors. In this work, we show that reprojections are crucial for reliable predictions. We provide an approach via closest-point projections that ensure consistency with this nonlinear manifold, which is strongly related to a Riemannian metric and maximum likelihood estimates. While these results are already novel for autonomous systems, we present our approach for parametric systems, providing the basis for data-driven bifurcation analysis and control applications.

This letter proposes a new approach for Inertial Measurement Unit (IMU) preintegration, a fundamental building block that can be leveraged in different optimization-based Inertial Navigation System (INS) localization solutions. Inspired by recent advances in equivariant theory applied to biased INSs, we derive a discrete-time formulation of the IMU preintegration on Gal(3)gal(3) , the left-trivialization of the tangent group of the Galilean group Gal(3) . We define a novel preintegration error that geometrically couples the navigation states and the bias leading to lower linearization error. Our method improves in consistency compared to existing preintegration approaches which treat IMU biases as a separate state-space. Extensive validation against state-of-the-art methods, both in simulation and with real-world IMU data, implementation in the Lie++ library, and open-source code are provided.

This paper investigates the problem of inertial navigation system (INS) filter design through the lens of symmetry. The extended Kalman filter (EKF) and its variants, have been the staple of INS filtering for 50 years; however, recent advances in inertial navigation systems have exploited matrix Lie group structure to design stochastic filters and state observers that have been shown to display superior performance compared to classical solutions. In this work we consider the case where a vehicle has an inertial measurement unit (IMU) and a global navigation satellite system (GNSS) receiver. We show that all the modern variants of the EKF for these sensors can be interpreted as the recently proposed Equivariant Filter (EqF) design methodology applied to different choices of symmetry group for the INS problem. This leads us to propose two new symmetries for the INS problem that have not been considered in the prior literature, and provide a discussion of the relative strengths and weaknesses of all the different algorithms. We believe the collection of symmetries that we present here capture all the sensible choices of symmetry for this problem and sensor suite, and that the analysis provided is indicative of the relative real-world performance potential of the different algorithms.

Galilean symmetry is the natural symmetry of inertial motion that underpins Newtonian physics. Although rigid-body symmetry is one of the most established and fundamental tools in robotics, there appears to be no comparable treatment of Galilean symmetry for a robotics audience. In this paper, we present a robotics-tailored exposition of Galilean symmetry that leverages the community's familiarity with and understanding of rigid-body transformations and pose representations. Our approach contrasts with common treatments in the physics literature that introduce Galilean symmetry as a stepping stone to Einstein's relativity. A key insight is that the Galilean matrix Lie group can be used to describe two different pose representations, Galilean frames, that use inertial velocity in the state definition, and extended poses, that use coordinate velocity. We provide three examples where applying the Galilean matrix Lie-group algebra to robotics problems is straightforward and yields significant insights: inertial navigation above the rotating Earth, manipulator kinematics, and sensor data fusion under temporal uncertainty. We believe that the time is right for the robotics community to benefit from rediscovering and extending this classical material and applying it to modern problems.

Event-based cameras are popular for tracking fast-moving objects due to their high temporal resolution, low latency, and high dynamic range. In this paper, we propose a novel algorithm for tracking event blobs using raw events asynchronously in real time. We introduce the concept of an event blob as a spatio-temporal likelihood of event occurrence where the conditional spatial likelihood is blob-like. Many real-world objects such as car headlights or any quickly moving foreground objects generate event blob data. The proposed algorithm uses a nearest neighbour classifier with a dynamic threshold criteria for data association coupled with an extended Kalman filter to track the event blob state. Our algorithm achieves highly accurate blob tracking, velocity estimation, and shape estimation even under challenging lighting conditions and high-speed motions (> 11000 pixels/s). The microsecond time resolution achieved means that the filter output can be used to derive secondary information such as time-to-contact or range estimation, that will enable applications to real-world problems such as collision avoidance in autonomous driving.

The trajectory tracking problem is a fundamental control task in the study of mechanical systems. A key construction in tracking control is the error or difference between an actual and desired trajectory. This construction also lies at the heart of observer design and recent advances in the study of equivariant systems have provided a template for global error construction that exploits the symmetry structure of a group action if such a structure exists. Hamiltonian systems are posed on the cotangent bundle of configuration space of a mechanical system and symmetries for the full cotangent bundle are not commonly used in geometric control theory. In this paper, we propose a group structure on the cotangent bundle of a Lie group and leverage this to define momentum and configuration errors for trajectory tracking drawing on recent work on equivariant observer design. We show that this error definition leads to error dynamics that are themselves “Euler-Poincare like” and use these to derive simple, almost global trajectory tracking control for fully-actuated Euler-Poincare systems on a Lie group state space.

Choosing a nonlinear state estimator for an application often involves a trade-off between local optimality (such as provided by an extended Kalman filter) and (almost-/semi-) global asymptotic stability (such as provided by a constructive observer design based on Lyapunov principles). This paper proposes a filter design methodology that is both global and optimal for a class of nonlinear systems. In particular, systems for which there is an embedding of the state-manifold into Euclidean space for which the measurement function is linear in the embedding space and for which there is a synchronous error construction. A novel observer is derived using the minimum energy filter design paradigm and exploiting the embedding coordinates to solve for the globally optimal solution exactly. The observer is demonstrated through an application to the problem of unit quaternion attitude estimation, by embedding the 3dimensional nonlinear system into a 4 -dimensional Euclidean space. Simulation results demonstrate that the state estimate remains optimal for all time and converges even with a very large initial error.