arXiv subjects

Pieter van Goor

Publications and source records attributed to Pieter van Goor.

At least 19 recordsLinked to original sources

arXiv2026

Equivariant Filter Design for Acoustic and Depth Aided Inertial Navigation Systems

Autonomous Underwater Vehicles (AUVs) navigating without GPS typically fuse inertial measurements with acoustic Doppler Velocity Log (DVL) velocities and pressure-derived depth. Posing the navigation state on a Lie group improves accuracy and consistency. However, state-of-the-art filters based on the Invariant Extended Kalman Filter (IEKF) append the Inertial Measurement Unit (IMU) biases as a Euclidean extension, which breaks the group-affine structure required for exact log-linear error dynamics, causing the reported covariance to degrade alongside the estimate. We apply the Tangent-Group (TG) symmetry, which carries the biases within the geometry of the state space, to derive an Equivariant Filter (EqF) for this system, leaving zero linearization error in the navigation states and second-order error only in the biases. We develop an equivariant output model for the DVL, whose update incurs only third-order linearization error, together with a direct pressure output. Monte Carlo simulations benchmark the TG-EqF against a Two-Frame-Group IEKF and a Multiplicative EKF. The TG-EqF reduces error by 18--25\% against both alternatives in each of attitude, velocity, and position. The main benefit is in the covariance it estimates: its Average Normalized Estimation Error Squared (ANEES) stays closer to its nominal value of one than that of the others. Offline analysis on AUV field data corroborates the findings of the simulations, demonstrating reduced position drift.

cs.RO↗

arXiv2026

The Iterative Equivariant Filter

This paper presents the iterative equivariant filter (IterEqF). The iterative extended Kalman filter (IterEKF) replaces the standard EKF correction step with an iterative correction step that is the Gauss-Newton solution to a nonlinear weighted least squares problem. The standard equivariant filter (EqF) exploits and respects the underlying symmetry of state estimation problems posed on homogeneous spaces and Lie groups. The motivation behind the IterEqF is to combine the features of both the IterEKF and the EqF, thus leading to a high-performance state estimation solution that is well suited to navigation problems. This paper derives the iteration procedure for the IterEqF update step and shows how the intrinsic nonlinearity of the approach naturally results in a `reset' of the filter's covariance into new coordinates. Monte-Carlo simulations of range-based localisation for a mobile robot demonstrate the improvement in performance relative to a standard EqF, especially during the transient convergence.

eess.SY↗

arXiv2026

A Weak Notion of Symmetry for Dynamical Systems

Many nonlinear dynamical systems exhibit symmetry, affording substantial benefits for control design, observer architecture, and data-driven control. While the classical notion of group invariance enables a cascade decomposition of the system into highly structured subsystems, it demands very rigid structure in the original system. Conversely, much more general notions (e.g., partial symmetry) have been shown to be sufficient for obtaining less-structured decompositions. In this work, we propose a middle ground termed "weak invariance", studying diffeomorphisms (resp., vector fields) that are group invariant up to a diffeomorphism of (resp., vector field on) the symmetry group. Remarkably, we prove that weak invariance implies that this diffeomorphism of (resp., vector field on) the symmetry group must be an automorphism (resp., group linear). Additionally, we demonstrate that a vector field is weakly invariant if and only if its flow is weakly invariant, where the associated group linear vector field generates the associated automorphisms. Finally, we show that weakly invariant systems admit a cascade decomposition in which the dynamics are group affine along the orbits. Weak invariance thus generalizes both classical invariance and the important class of group affine dynamical systems on Lie groups, laying a foundation for new methods of symmetry-informed control and observer design.

eess.SY↗

arXiv2026

Tracking Control for a Dynamic Model of an Underwater Submersible

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.

eess.SY↗

arXiv2026

Equivariant Observer Design on SL(3) for Image Intensity-Based Homography Estimation

This paper addresses the problem of homography estimation using a nonlinear observer designed on the Lie group $\mathbf{SL}(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.

eess.SY↗

arXiv2026

A Comparative Study of INDI and NDI with Nonlinear Disturbance Observer for Aerial Robotics

This work presents a simulation-based comparative robustness analysis of Incremental Nonlinear Dynamic Inversion (INDI) and Nonlinear Dynamic Inversion augmented with a nonlinear disturbance observer (NDI+NDO) for fully actuated aerial robots. A systematic simulation campaign across representative operating scenarios is conducted, where we compare tracking performance, robustness, control effort, under parametric variations, external disturbances, and measurement noise. Results show that INDI demonstrates stronger robustness in several model-mismatch and combined-stress cases, while NDI+NDO primarily matches nominal performance but exhibits greater sensitivity under several non-ideal conditions. These findings provide practical guidance on the relative strengths and limitations of incremental and observer-based inversion strategies for aerial robotic applications.

cs.RO↗

arXiv2026

Synchronous Observer Design for Landmark-Inertial SLAM with Magnetometer and Intermittent GNSS Measurements

In Landmark-Inertial Simultaneous Localisation and Mapping (LI-SLAM), the positions of landmarks in the environment and the robot's pose relative to these landmarks are estimated using landmark position measurements, and measurements from the Inertial Measurement Unit (IMU). However, the robot and landmark positions in the inertial frame, and the yaw of the robot, are not observable in LI-SLAM. This paper proposes a nonlinear observer for LI-SLAM that overcomes the observability constraints with the addition of intermittent GNSS position and magnetometer measurements. The full-state error dynamics of the proposed observer is shown to be both almost-globally asymptotically stable and locally exponentially stable, and this is validated using simulations.

eess.SY↗

arXiv2026

The Geometry of Coordinated Trajectories for Non-stop Flying Carriers Holding a Cable-Suspended Load

This work considers the problem of using multiple aerial carriers to hold a cable-suspended load while remaining in periodic motion at all times. Using a novel differential geometric perspective, it is shown that the problem may be recast as that of finding an immersion of the unit circle into the smooth manifold of admissible configurations. Additionally, this manifold is shown to be path connected under a mild assumption on the attachment points of the carriers to the load. Based on these ideas, a family of simple linear solutions to the original problems is presented that overcomes the constraints of alternative solutions previously proposed in the literature. Simulation results demonstrate the flexibility of the theory in identifying suitable solutions.

eess.SY↗

arXiv2026

Input Dexterity and Output Negotiation in Feedback-Linearizable Nonlinear Systems

We introduce a task-relative taxonomy of actuator inputs for nonlinear systems within the input-output feedback-linearization framework. Given a flat output specifying the task, inputs are classified as essential, redundant, or dexterity: essential inputs are required for exact linearization, redundant inputs can be removed without effect, and dexterity inputs can be deactivated while preserving exact linearization of a reduced task. We show that a subset is dexterity if and only if, under a suitable dynamic prolongation, it can appear as additional output channels (flat-input complement) on a common validity set. Whenever a family of systems obtained by (de)activating dexterity inputs admits a common prolongation, the family can be interpreted as a single prolonged system endowed with different output selections. This enables a unified linearizing controller that negotiates between full and reduced tasks without transients on shared outputs under compatibility and dwell-time conditions. Simulations on a fully actuated aerial platform illustrate graceful task downgrades from six-dimensional pose tracking as lateral-force channels are deactivated.

eess.SY↗

arXiv2026

Switching Among Feedback-Linearizing Output Sets (Melds): Dwell-Time and Compatibility Guarantees

We study switching among multiple square selections of output functions (melds) drawn from a deck of candidate outputs for nonlinear systems that are static feedback linearizable via outputs. Fixing an operating point, each meld induces a distinct feedback-linearizing coordinate chart defined on a common neighborhood. Switching between melds therefore produces state-dependent coordinate mismatches that are not captured by classical switched-system analyses. We quantify this effect through Lipschitz bounds on the cross-chart maps over a compact safe set and introduce a reference-compatibility constant that measures mismatch among reference families across melds. We derive an explicit dwell-time condition depending on controller decay rates and the compatibility constant, that guarantees exponential decay of the active-output tracking errors between switches, seamless tracking of outputs shared by consecutive melds, and uniform boundedness of the state error within the safe set. A planar 3R manipulator illustrates the results.

cs.RO↗

arXiv2026

On Data-Driven Unbiased Predictors using the Koopman Operator

The Koopman operator and its data-driven approximations, such as extended dynamic mode decomposition (EDMD), are widely used for analysing, modelling, and controlling nonlinear dynamical systems. However, when the true Koopman eigenfunctions cannot be identified from finite data, multi-step predictions may suffer from structural inaccuracies and systematic bias. To address this issue, we analyse the first and second moments of the multi-step prediction residual. By decomposing the residual into contributions from the one-step approximation error and the propagation of accumulated inaccuracies, we derive a closed-form expression characterising these effects. This analysis enables the development of a novel and computationally efficient algorithm that enforces unbiasedness and reduces variance in the resulting predictor. The proposed method is validated in numerical simulations, showing improved uncertainty properties compared to standard EDMD. These results lay the foundation for uncertainty-aware and unbiased Koopman-based prediction frameworks that can be extended to controlled and stochastic systems.

math.DS↗

arXiv2026

Constructive Observer Design for Visual Simultaneous Localisation and Mapping

Visual Simultaneous Localisation and Mapping (VSLAM) is a well-known problem in robotics with a large range of applications. This paper presents a novel approach to VSLAM by lifting the observer design to a novel Lie group on which the system output is equivariant. The perspective gained from this analysis facilitates the design of a non-linear observer with almost semi-globally asymptotically stable error dynamics. Simulations are provided to illustrate the behaviour of the proposed observer and experiments on data gathered using a fixed-wing UAV flying outdoors demonstrate its performance.

eess.SY↗

arXiv2026

Constructive Equivariant Observer Design for Inertial Velocity-Aided Attitude

Inertial Velocity-Aided Attitude (VAA), the estimation of the velocity and attitude of a vehicle using gyroscope, accelerometer, and inertial-frame velocity (e.g. GPS velocity) measurements, is an important problem in the control of Remotely Piloted Aerial Systems (RPAS). Existing solutions provide limited stability guarantees, relying on local linearisation, high gain design, or assuming specific trajectories such as constant acceleration of the vehicle. This paper proposes a novel non-linear observer for inertial VAA with almost globally asymptotically and locally exponentially stable error dynamics. The approach exploits Lie group symmetries of the system dynamics to construct a globally valid correction term. To the authors' knowledge, this construction is the first observer to provide almost global convergence for the inertial VAA problem. The observer performance is verified in simulation, where it is shown that the estimation error converges to zero even with an extremely poor initial condition.

eess.SY↗

arXiv2026

Equivariant Filter (EqF)

The kinematics of many systems encountered in robotics, mechatronics, and avionics are naturally posed on homogeneous spaces; that is, their state lies in a smooth manifold equipped with a transitive Lie group symmetry. This paper proposes a novel filter, the Equivariant Filter (EqF), by posing the observer state on the symmetry group, linearising global error dynamics derived from the equivariance of the system, and applying extended Kalman filter design principles. We show that equivariance of the system output can be exploited to reduce linearisation error and improve filter performance. Simulation experiments of an example application show that the EqF significantly outperforms the extended Kalman filter and that the reduced linearisation error leads to a clear improvement in performance.

eess.SY↗

arXiv2026

EqVIO: An Equivariant Filter for Visual Inertial Odometry

Visual-Inertial Odometry (VIO) is the problem of estimating a robot's trajectory by combining information from an inertial measurement unit (IMU) and a camera, and is of great interest to the robotics community. This paper develops a novel Lie group symmetry for the VIO problem and applies the recently proposed equivariant filter. The proposed symmetry is compatible with the invariance of the VIO reference frame, leading to improved filter consistency. The bias-free IMU dynamics are group-affine, ensuring that filter linearisation errors depend only on the bias estimation error and measurement noise. Furthermore, visual measurements are equivariant with respect to the symmetry, enabling the application of the higher-order equivariant output approximation to reduce approximation error in the filter update equation. As a result, the equivariant filter (EqF) based on this Lie group is a consistent estimator for VIO with lower linearisation error in the propagation of state dynamics and a higher order equivariant output approximation than standard formulations. Experimental results on the popular EuRoC and UZH FPV datasets demonstrate that the proposed system outperforms other state-of-the-art VIO algorithms in terms of both speed and accuracy.

cs.RO↗

arXiv2025

Equivariant Tracking Control for Fully Actuated Mechanical Systems on Matrix Lie Groups

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.

eess.SY↗

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↗