Uncertainty Modelling and Robust Observer Synthesis using the Koopman Operator

This paper proposes a robust nonlinear observer synthesis method for a population of systems modelled using the Koopman operator. The Koopman operator allows nonlinear systems to be rewritten as infinite-dimensional linear systems. A finite-dimensional approximation of the Koopman operator can be identified directly from data, yielding an approximately linear model of a nonlinear system. The proposed observer synthesis method is made possible by this linearity that in turn allows uncertainty within a population of Koopman models to be quantified in the frequency domain. Using this uncertainty model, linear robust control techniques are used to synthesize robust nonlinear Koopman observers. A population of several dozen motor drives is used to experimentally demonstrate the proposed method. Manufacturing variation is characterized in the frequency domain, and a robust Koopman observer is synthesized using mixed $\mathcal{H}_2$-$\mathcal{H}_\infty$ optimal control.

The Harmonic Exponential Filter for Nonparametric Estimation on Motion Groups

Bayesian estimation is a vital tool in robotics as it allows systems to update the belief of the robot state using incomplete information from noisy sensors. To render the state estimation problem tractable, many systems assume that the motion and measurement noise, as well as the state distribution, are all unimodal and Gaussian. However, there are numerous scenarios and systems that do not comply with these assumptions. Existing non-parametric filters that are used to model multimodal distributions have drawbacks that limit their ability to represent a diverse set of distributions. In this paper, we introduce a novel approach to nonparametric Bayesian filtering to cope with multimodal distributions using harmonic exponential distributions. This approach leverages two key insights of harmonic exponential distributions: a) the product of two distributions can be expressed as the element-wise addition of their log-likelihood Fourier coefficients, and b) the convolution of two distributions can be efficiently computed as the tensor product of their Fourier coefficients. These observations enable the development of an efficient and exact solution to the Bayes filter up to the band limit of a Fourier transform. We demonstrate our filter's superior performance compared with established nonparametric filtering methods across a range of simulated and real-world localization tasks.

A Hessian for Gaussian Mixture Likelihoods in Nonlinear Least Squares

This paper proposes a novel Hessian approximation for Maximum a Posteriori estimation problems in robotics involving Gaussian mixture likelihoods. The proposed Hessian leads to better convergence properties. Previous approaches manipulate the Gaussian mixture likelihood into a form that allows the problem to be represented as a nonlinear least squares (NLS) problem. However, they result in an inaccurate Hessian approximation due to additional nonlinearities that are not accounted for in NLS solvers. The proposed Hessian approximation is derived by setting the Hessians of the Gaussian mixture component errors to zero, which is the same starting point as for the Gauss-Newton Hessian approximation for NLS, and using the chain rule to account for additional nonlinearities. The proposed Hessian approximation is more accurate, resulting in improved convergence properties that are demonstrated on simulated and real-world experiments. A method to maintain compatibility with existing solvers, such as ceres, is also presented. Accompanying software and supplementary material can be found at https://github.com/decargroup/hessian_sum_mixtures.

Laser-to-Vehicle Extrinsic Calibration in Low-Observability Scenarios for Subsea Mapping

Laser line scanners are increasingly being used in the subsea industry for high-resolution mapping and infrastructure inspection. However, calibrating the 3D pose of the scanner relative to the vehicle is a perennial source of confusion and frustration for industrial surveyors. This work describes three novel algorithms for laser-to-vehicle extrinsic calibration using naturally occurring features. Each algorithm makes a different assumption on the quality of the vehicle trajectory estimate, enabling good calibration results in a wide range of situations. A regularization technique is used to address low-observability scenarios frequently encountered in practice with large, rotationally stable subsea vehicles. Experimental results are provided for two field datasets, including the recently discovered wreck of the Endurance.

Optimal Robot Formations: Balancing Range-Based Observability and User-Defined Configurations

This paper introduces a set of customizable and novel cost functions that enable the user to easily specify desirable robot formations, such as a ``high-coverage'' infrastructure-inspection formation, while maintaining high relative pose estimation accuracy. The overall cost function balances the need for the robots to be close together for good ranging-based relative localization accuracy and the need for the robots to achieve specific tasks, such as minimizing the time taken to inspect a given area. The formations found by minimizing the aggregated cost function are evaluated in a coverage path planning task in simulation and experiment, where the robots localize themselves and unknown landmarks using a simultaneous localization and mapping algorithm based on the extended Kalman filter. Compared to an optimal formation that maximizes ranging-based relative localization accuracy, these formations significantly reduce the time to cover a given area with minimal impact on relative pose estimation accuracy.

Navigation and Control of Unconventional VTOL UAVs in Forward-Flight with Explicit Wind Velocity Estimation

This paper presents a solution for the state estimation and control problems for a class of unconventional vertical takeoff and landing (VTOL) UAVs operating in forward-flight conditions. A tightly-coupled state estimation approach is used to estimate the aircraft navigation states, sensor biases, and the wind velocity. State estimation is done within a matrix Lie group framework using the Invariant Extended Kalman Filter (IEKF), which offers several advantages compared to standard multiplicative EKFs traditionally used in aerospace and robotics problems. An SO(3)- based attitude controller is employed, leading to a single attitude control law without a separate sideslip control loop. A control allocator is used to determine how to use multiple, possibly redundant, actuators to produce the desired control moments. The wind velocity estimates are used in the attitude controller and the control allocator to improve performance. A numerical example is considered using a sample VTOL tailsitter-type UAV with four control surfaces. Monte-Carlo simulations demonstrate robustness of the proposed control and estimation scheme to various initial conditions, noise levels, and flight trajectories.

The Invariant Rauch-Tung-Striebel Smoother

This paper presents an invariant Rauch-Tung- Striebel (IRTS) smoother applicable to systems with states that are an element of a matrix Lie group. In particular, the extended Rauch-Tung-Striebel (RTS) smoother is adapted to work within a matrix Lie group framework. The main advantage of the invariant RTS (IRTS) smoother is that the linearization of the process and measurement models is independent of the state estimate resulting in state-estimate-independent Jacobians when certain technical requirements are met. A sample problem is considered that involves estimation of the three dimensional pose of a rigid body on SE(3), along with sensor biases. The multiplicative RTS (MRTS) smoother is also reviewed and is used as a direct comparison to the proposed IRTS smoother using experimental data. Both smoothing methods are also compared to invariant and multiplicative versions of the Gauss-Newton approach to solving the batch state estimation problem.

navlie: A Python Package for State Estimation on Lie Groups

The ability to rapidly test a variety of algorithms for an arbitrary state estimation task is valuable in the prototyping phase of navigation systems. Lie group theory is now mainstream in the robotics community, and hence estimation prototyping tools should allow state definitions that belong to manifolds. A new package, called navlie, provides a framework that allows a user to model a large class of problems by implementing a set of classes complying with a generic interface. Once accomplished, navlie provides a variety of on-manifold estimation algorithms that can run directly on these classes. The package also provides a built-in library of common models, as well as many useful utilities. The open-source project can be found at https://github.com/decargroup/navlie.

IMU Preintegration for Multi-Robot Systems in the Presence of Bias and Communication Constraints

This document is in supplement to the paper titled "Multi-Robot Relative Pose Estimation and IMU Preintegration Using Passive UWB Transceivers", available at [1]. The purpose of this document is to show how IMU biases can be incorporated into the framework presented in [1], while maintaining the differential Sylvester equation form of the process model.

Bayesian Filtering for Homography Estimation

This paper considers homography estimation in a Bayesian filtering framework using rate gyro and camera measurements. The use of rate gyro measurements facilitates a more reliable estimate of homography in the presence of occlusions, while a Bayesian filtering approach generates both a homography estimate along with an uncertainty. Uncertainty information opens the door to adaptive filtering approaches, post-processing procedures, and safety protocols. In particular, herein an iterative extended Kalman filter and an interacting multiple model (IMM) filter are tested using both simulated and experimental datasets. The IMM is shown to have good consistency properties and better overall performance when compared to the state-of-the-art homography nonlinear deterministic observer in both simulations and experiments.