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.

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.

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.

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.

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.

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.

Magnetic Navigation using Attitude-Invariant Magnetic Field Information for Loop Closure Detection

Indoor magnetic fields are a combination of Earth's magnetic field and disruptions induced by ferromagnetic objects, such as steel structural components in buildings. As a result of these disruptions, pervasive in indoor spaces, magnetic field data is often omitted from navigation algorithms in indoor environments. This paper leverages the spatially-varying disruptions to Earth's magnetic field to extract positional information for use in indoor navigation algorithms. The algorithm uses a rate gyro and an array of four magnetometers to estimate the robot's pose. Additionally, the magnetometer array is used to compute attitude-invariant measurements associated with the magnetic field and its gradient. These measurements are used to detect loop closure points. Experimental results indicate that the proposed approach can estimate the pose of a ground robot in an indoor environment within meter accuracy.

Combining DVL-INS and Laser-Based Loop Closures in a Batch Estimation Framework for Underwater Positioning

Correcting gradual position drift is a challenge in long-term subsea navigation. Though highly accurate, modern inertial navigation system (INS) estimates will drift over time due to the accumulated effects of sensor noise and biases, even with acoustic aiding from a Doppler velocity log (DVL). The raw sensor measurements and estimation algorithms used by the DVL-aided INS are often proprietary, which restricts the fusion of additional sensors that could bound navigation drift over time. In this letter, the raw sensor measurements and their respective covariances are estimated from the DVL-aided INS output using semidefinite programming tools. The estimated measurements are then augmented with laser-based loop-closure measurements in a batch state estimation framework to correct planar position errors. The heading uncertainty from the DVL-aided INS is also considered in the estimation of the updated positions. The pipeline is tested in simulation and on experimental field data. The proposed methodology reduces the long-term navigation drift by more than 30 times compared to the DVL-aided INS estimate.