arXiv subjects

Pieter van Goor

Publications and source records attributed to Pieter van Goor.

At least 37 records · Page 2Linked to original sources

arXiv2025

Shaping the Koopman dictionary by learning on the Grassmannian

Extended dynamic mode decomposition (EDMD) is a powerful tool to construct linear predictors of nonlinear dynamical systems by approximating the action of the Koopman operator on a subspace spanned by finitely many observable functions. However, its accuracy heavily depends on the choice of the observables, which remains a challenge. We propose a systematic framework to identify and shape observable dictionaries, reduce projection errors, and achieve approximately invariant subspaces. To this end, we leverage optimisation on the Grassmann manifold and exploit inherent geometric properties for computational efficiency. Numerical results demonstrate improved prediction accuracy and efficiency. In conclusion, we propose a novel approach to efficiently shape the Koopman dictionary using differential-geometric concepts for optimisation on manifolds resulting in enhanced data-driven Koopman surrogates for nonlinear dynamical systems.

math.DS↗

arXiv2025

Synchronous Observer Design for Landmark-Inertial SLAM with Almost-Global Convergence

Landmark Inertial Simultaneous Localisation and Mapping (LI-SLAM) is the problem of estimating the locations of landmarks in the environment and the robot's pose relative to those landmarks using landmark position measurements and measurements from Inertial Measurement Unit (IMU). This paper proposes a nonlinear observer for LI-SLAM posed in continuous time and analyses the observer in a base space that encodes all the observable states of LI-SLAM. The local exponential stability and almost-global asymptotic stability of the error dynamics in base space is established in the proof section and validated using simulations.

eess.SY↗

arXiv2025

The Difference between the Left and Right Invariant Extended Kalman Filter

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.

cs.RO↗

arXiv2025

The Geometry of Extended Kalman Filters on Manifolds with Affine Connection

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.

eess.SY↗

arXiv2025

Equivariant Symmetries for Inertial Navigation Systems

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 explore various symmetries of inertial navigation system, including two novel symmetries that have not been considered in the prior literature, and provide a discussion of the relative strengths and weaknesses of these symmetries in the context of filter design. We show that all the modern variants of the EKF for inertial navigation can be interpreted as the recently proposed equivariant filter (EqF) design methodology applied to different choices of symmetry group for the INS problem. As a direct application of the symmetries presented, we address the filter design problem for a vehicle equipped with an inertial measurement unit (IMU) and a global navigation satellite system (GNSS) receiver, providing a comparative analysis of different modern filter solutions. We believe the collection of symmetries that we present here capture all the sensible choices of symmetry for this problem, and that the analysis provided is indicative of the relative real-world performance potential of the different algorithms for trajectories ensuring full state observability.

cs.RO↗

arXiv2025

Synchronous Models and Fundamental Systems in Observer Design

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.

eess.SY↗

arXiv2025

Synchronous Observer Design for Inertial Navigation Systems with Almost-Global Convergence

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 Global Navigation Satellite Systems (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 a synchronous observer architecture; that is, an observer architecture for which the observer error is stationary if the 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.

eess.SY↗

arXiv2025

Unified Feedback Linearization for Nonlinear Systems with Dexterous and Energy-Saving Modes

Systems with a high number of inputs compared to the degrees of freedom (e.g. a mobile robot with Mecanum wheels) often have a minimal set of energy-efficient inputs needed to achieve a main task (e.g. position tracking) and a set of energy-intense inputs needed to achieve an additional auxiliary task (e.g. orientation tracking). This letter presents a unified control scheme, derived through feedback linearization, that can switch between two modes: an energy-saving mode, which tracks the main task using only the energy-efficient inputs while forcing the energy-intense inputs to zero, and a dexterous mode, which also uses the energy-intense inputs to track the auxiliary task as needed. The proposed control guarantees the exponential tracking of the main task and that the dynamics associated with the main task evolve independently of the a priori unknown switching signal. When the control is operating in dexterous mode, the exponential tracking of the auxiliary task is also guaranteed. Numerical simulations on an omnidirectional Mecanum wheel robot validate the effectiveness of the proposed approach and demonstrate the effect of the switching signal on the exponential tracking behavior of the main and auxiliary tasks.

cs.RO↗

arXiv2025

Equivariant Filter Design for Range-only SLAM

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.

cs.RO↗

arXiv2024

Global Minimum Energy State Estimation for Embedded Nonlinear Systems with Symmetry

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 3-dimensional 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.

eess.SY↗

arXiv2024

Asynchronous Blob Tracker for Event Cameras

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.

cs.CV↗

arXiv2024

Geometric Data Fusion for Collaborative Attitude Estimation

In this paper, we consider the collaborative attitude estimation problem for a multi-agent system. The agents are equipped with sensors that provide directional measurements and relative attitude measurements. We present a bottom-up approach where each agent runs an extended Kalman filter (EKF) locally using directional measurements and augments this with relative attitude measurements provided by neighbouring agents. The covariance estimates of the relative attitude measurements are geometrically corrected to compensate for relative attitude between the agent that makes the measurement and the agent that uses the measurement before being fused with the local estimate using the convex combination ellipsoid (CCE) method to avoid data incest. Simulations are undertaken to numerically evaluate the performance of the proposed algorithm.

eess.SY↗

arXiv2024

A Geometric Perspective on Fusing Gaussian Distributions on Lie Groups

Stochastic inference on Lie groups plays a key role in state estimation problems such as; inertial navigation, visual inertial odometry, pose estimation in virtual reality, etc. A key problem is fusing independent concentrated Gaussian distributions defined at different reference points on the group. In this paper we approximate distributions at different points in the group in a single set of exponential coordinates and then use classical Gaussian fusion to obtain the fused posteriori in those coordinates. We consider several approximations including the exact Jacobian of the change of coordinate map, first and second order Taylor's expansions of the Jacobian, and parallel transport with and without curvature correction associated with the underlying geometry of the Lie group. Preliminary results on SO(3) demonstrate that a novel approximation using parallel transport with curvature correction achieves similar accuracy to the state-of-the-art optimisation based algorithms at a fraction of the computational cost.

eess.SY↗

arXiv2024

Exploiting Equivariance in the Design of Tracking Controllers for Euler-Poincare Systems on Matrix Lie Groups

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.

eess.SY↗

arXiv2023

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

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 visual-inertial 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 $\mathbf{SE}_2(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., long-duration missions. We evaluate our approach with a multitude of different experiments using three different prominent real-world datasets.

cs.RO↗

arXiv2023

Exploiting spatial group error and synchrony for a unicycle tracking controller

Trajectory tracking for the kinematic unicycle has been heavily studied for several decades. The unicycle admits a natural $\SE(2)$ symmetry, a key structure exploited in many of the most successful nonlinear controllers in the literature. To the author's knowledge however, all prior work has used a body-fixed, or left-invariant, group error formulation for the study of the tracking problem. In this paper, we consider the spatial, or right-invariant, group error in the design of a tracking controller for the kinematic unicycle. We provide a physical interpretation of the right-invariant error and go on to show that the associated error dynamics are drift-free, a property that is not true for the body-fixed error. We exploit this property to propose a simple nonlinear control scheme for the kinematic unicycle and prove almost-global asymptotic stability of this control scheme for a class of persistently exciting trajectories. We also verify performance of this control scheme in simulation for an example trajectory.

eess.SY↗

arXiv2023

A Note on the Extended Kalman Filter on a Manifold

The kinematics of many control systems, especially in the robotics field, naturally live on smooth manifolds. Most classical state-estimation algorithms, including the extended Kalman filter, are posed on Euclidean space. Although any filter algorithm can be adapted to a manifold setting by implementing it in local coordinates and ignoring the geometric structure, it has always been clear that there would be advantages in taking the geometric structure into consideration in developing the algorithm. In this paper, we argue that the minimum geometric structure required to adapt the extended Kalman filter to a manifold is that of an affine connection. With this structure, we show that a naive coordinate implementation of the EKF fails to account for geometry of the manifold in the update step and in the reset step. We provide geometric modifications to the classical EKF based on parallel transport of the measurement covariance (for the update) and a-posteriori state covariance (for the reset) that address these limitations. Preliminary results for attitude estimation with two directional measurements demonstrate that the proposed modifications significantly improve the transient behavior of the filter.

eess.SY↗

arXiv2023

Constructive Equivariant Observer Design for Inertial Navigation

Inertial Navigation Systems (INS) are algorithms that fuse inertial measurements of angular velocity and specific acceleration with supplementary sensors including GNSS and magnetometers to estimate the position, velocity and attitude, or extended pose, of a vehicle. The industry-standard extended Kalman filter (EKF) does not come with strong stability or robustness guarantees and can be subject to catastrophic failure. This paper exploits a Lie group symmetry of the INS dynamics to propose the first nonlinear observer for INS with error dynamics that are almost-globally asymptotically and locally exponentially stable, independently of the chosen gains. The observer is aided only by a GNSS measurement of position. As expected, the convergence guarantee depends on persistence of excitation of the vehicle's specific acceleration in the inertial frame. Simulation results demonstrate the observer's performance and its ability to converge from extreme errors in the initial state estimates.

eess.SY↗