-
A Hessian for Gaussian Mixture Likelihoods in Nonlinear Least Squares
Authors:
Vassili Korotkine,
Mitchell Cohen,
James Richard Forbes
Abstract:
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 inac…
▽ More
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.
△ Less
Submitted 8 April, 2024;
originally announced April 2024.
-
Optimal Robot Formations: Balancing Range-Based Observability and User-Defined Configurations
Authors:
Syed Shabbir Ahmed,
Mohammed Ayman Shalaby,
Jerome Le Ny,
James Richard Forbes
Abstract:
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…
▽ More
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 map** 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.
△ Less
Submitted 1 March, 2024;
originally announced March 2024.
-
Navigation and Control of Unconventional VTOL UAVs in Forward-Flight with Explicit Wind Velocity Estimation
Authors:
Mitchell Cohen,
James Richard Forbes
Abstract:
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 Invaria…
▽ More
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.
△ Less
Submitted 29 February, 2024;
originally announced March 2024.
-
The Invariant Rauch-Tung-Striebel Smoother
Authors:
Niels van der Laan,
Mitchell Cohen,
Jonathan Arsenault,
James Richard Forbes
Abstract:
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…
▽ More
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.
△ Less
Submitted 29 February, 2024;
originally announced March 2024.
-
Laser-to-Vehicle Extrinsic Calibration in Low-Observability Scenarios for Subsea Map**
Authors:
Thomas Hitchcox,
James Richard Forbes
Abstract:
Laser line scanners are increasingly being used in the subsea industry for high-resolution map** 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.…
▽ More
Laser line scanners are increasingly being used in the subsea industry for high-resolution map** 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.
△ Less
Submitted 8 March, 2024; v1 submitted 22 February, 2024;
originally announced February 2024.
-
Gaussian-Sum Filter for Range-based 3D Relative Pose Estimation in the Presence of Ambiguities
Authors:
Syed S. Ahmed,
Mohammed A. Shalaby,
Charles C. Cossette,
Jerome Le Ny,
James R. Forbes
Abstract:
Multi-robot systems must have the ability to accurately estimate relative states between robots in order to perform collaborative tasks, possibly with no external aiding. Three-dimensional relative pose estimation using range measurements oftentimes suffers from a finite number of non-unique solutions, or ambiguities. This paper: 1) identifies and accurately estimates all possible ambiguities in 2…
▽ More
Multi-robot systems must have the ability to accurately estimate relative states between robots in order to perform collaborative tasks, possibly with no external aiding. Three-dimensional relative pose estimation using range measurements oftentimes suffers from a finite number of non-unique solutions, or ambiguities. This paper: 1) identifies and accurately estimates all possible ambiguities in 2D; 2) treats them as components of a Gaussian mixture model; and 3) presents a computationally-efficient estimator, in the form of a Gaussian-sum filter (GSF), to realize range-based relative pose estimation in an infrastructure-free, 3D, setup. This estimator is evaluated in simulation and experiment and is shown to avoid divergence to local minima induced by the ambiguous poses. Furthermore, the proposed GSF outperforms an extended Kalman filter, demonstrates similar performance to the computationally-demanding particle filter, and is shown to be consistent.
△ Less
Submitted 13 February, 2024;
originally announced February 2024.
-
navlie: A Python Package for State Estimation on Lie Groups
Authors:
Charles Champagne Cossette,
Mitchell Cohen,
Vassili Korotkine,
Arturo del Castillo Bernal,
Mohammed Ayman Shalaby,
James Richard Forbes
Abstract:
The ability to rapidly test a variety of algorithms for an arbitrary state estimation task is valuable in the prototy** phase of navigation systems. Lie group theory is now mainstream in the robotics community, and hence estimation prototy** 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 clas…
▽ More
The ability to rapidly test a variety of algorithms for an arbitrary state estimation task is valuable in the prototy** phase of navigation systems. Lie group theory is now mainstream in the robotics community, and hence estimation prototy** 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.
△ Less
Submitted 24 October, 2023;
originally announced October 2023.
-
Bayesian Filtering for Homography Estimation
Authors:
Arturo Del Castillo Bernal,
Philippe Decoste,
James Richard Forbes
Abstract:
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 appro…
▽ More
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.
△ Less
Submitted 16 October, 2023;
originally announced October 2023.
-
IMU Preintegration for Multi-Robot Systems in the Presence of Bias and Communication Constraints
Authors:
Mohammed Ayman Shalaby,
Charles Champagne Cossette,
Jerome Le Ny,
James Richard Forbes
Abstract:
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.
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.
△ Less
Submitted 16 October, 2023; v1 submitted 12 October, 2023;
originally announced October 2023.
-
STAR-loc: Dataset for STereo And Range-based localization
Authors:
Frederike Dümbgen,
Mohammed A. Shalaby,
Connor Holmes,
Charles C. Cossette,
James R. Forbes,
Jerome Le Ny,
Timothy D. Barfoot
Abstract:
This document contains a detailed description of the STAR-loc dataset. For a quick starting guide please refer to the associated Github repository (https://github.com/utiasASRL/starloc). The dataset consists of stereo camera data (rectified/raw images and inertial measurement unit measurements) and ultra-wideband (UWB) data (range measurements) collected on a sensor rig in a Vicon motion capture a…
▽ More
This document contains a detailed description of the STAR-loc dataset. For a quick starting guide please refer to the associated Github repository (https://github.com/utiasASRL/starloc). The dataset consists of stereo camera data (rectified/raw images and inertial measurement unit measurements) and ultra-wideband (UWB) data (range measurements) collected on a sensor rig in a Vicon motion capture arena. The UWB anchors and visual landmarks (Apriltags) are of known position, so the dataset can be used for both localization and Simultaneous Localization and Map** (SLAM).
△ Less
Submitted 11 September, 2023;
originally announced September 2023.
-
Data-Driven Batch Localization and SLAM Using Koopman Linearization
Authors:
Zi Cong Guo,
Frederike Dümbgen,
James R. Forbes,
Timothy D. Barfoot
Abstract:
We present a framework for model-free batch localization and SLAM. We use lifting functions to map a control-affine system into a high-dimensional space, where both the process model and the measurement model are rendered bilinear. During training, we solve a least-squares problem using groundtruth data to compute the high-dimensional model matrices associated with the lifted system purely from da…
▽ More
We present a framework for model-free batch localization and SLAM. We use lifting functions to map a control-affine system into a high-dimensional space, where both the process model and the measurement model are rendered bilinear. During training, we solve a least-squares problem using groundtruth data to compute the high-dimensional model matrices associated with the lifted system purely from data. At inference time, we solve for the unknown robot trajectory and landmarks through an optimization problem, where constraints are introduced to keep the solution on the manifold of the lifting functions. The problem is efficiently solved using a sequential quadratic program (SQP), where the complexity of an SQP iteration scales linearly with the number of timesteps. Our algorithms, called Reduced Constrained Koopman Linearization Localization (RCKL-Loc) and Reduced Constrained Koopman Linearization SLAM (RCKL-SLAM), are validated experimentally in simulation and on two datasets: one with an indoor mobile robot equipped with a laser rangefinder that measures range to cylindrical landmarks, and one on a golf cart equipped with RFID range sensors. We compare RCKL-Loc and RCKL-SLAM with classic model-based nonlinear batch estimation. While RCKL-Loc and RCKL-SLAM have similar performance compared to their model-based counterparts, they outperform the model-based approaches when the prior model is imperfect, showing the potential benefit of the proposed data-driven technique.
△ Less
Submitted 8 September, 2023;
originally announced September 2023.
-
Magnetic Navigation using Attitude-Invariant Magnetic Field Information for Loop Closure Detection
Authors:
Natalia Pavlasek,
Charles Champagne Cossette,
David Roy-Guay,
James Richard Forbes
Abstract:
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…
▽ More
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.
△ Less
Submitted 5 September, 2023;
originally announced September 2023.
-
Combining DVL-INS and Laser-Based Loop Closures in a Batch Estimation Framework for Underwater Positioning
Authors:
Amro Al-Baali,
Thomas Hitchcox,
James Richard Forbes
Abstract:
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, w…
▽ More
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.
△ Less
Submitted 10 July, 2023;
originally announced July 2023.
-
An Adaptive Graduated Nonconvexity Loss Function for Robust Nonlinear Least Squares Solutions
Authors:
Kyungmin Jung,
Thomas Hitchcox,
James Richard Forbes
Abstract:
Many problems in robotics, such as estimating the state from noisy sensor data or aligning two point clouds, can be posed and solved as least-squares problems. Unfortunately, vanilla nonminimal solvers for least-squares problems are notoriously sensitive to outliers. As such, various robust loss functions have been proposed to reduce the sensitivity to outliers. Examples of loss functions include…
▽ More
Many problems in robotics, such as estimating the state from noisy sensor data or aligning two point clouds, can be posed and solved as least-squares problems. Unfortunately, vanilla nonminimal solvers for least-squares problems are notoriously sensitive to outliers. As such, various robust loss functions have been proposed to reduce the sensitivity to outliers. Examples of loss functions include pseudo-Huber, Cauchy, and Geman-McClure. Recently, these loss functions have been generalized into a single loss function that enables the best loss function to be found adaptively based on the distribution of the residuals. However, even with the generalized robust loss function, most nonminimal solvers can only be solved locally given a prior state estimate due to the nonconvexity of the problem. The first contribution of this paper is to combine graduated nonconvexity (GNC) with the generalized robust loss function to solve least-squares problems without a prior state estimate and without the need to specify a loss function. Moreover, existing loss functions, including the generalized loss function, are based on Gaussian-like distribution. However, residuals are often defined as the squared norm of a multivariate error and distributed in a Chi-like fashion. The second contribution of this paper is to apply a norm-aware adaptive robust loss function within a GNC framework. The proposed approach enables a GNC formulation of a generalized loss function such that GNC can be readily applied to a wider family of loss functions. Furthermore, simulations and experiments demonstrate that the proposed method is more robust compared to non-GNC counterparts, and yields faster convergence times compared to other GNC formulations.
△ Less
Submitted 10 May, 2024; v1 submitted 11 May, 2023;
originally announced May 2023.
-
Decentralized State Estimation: An Approach using Pseudomeasurements and Preintegration
Authors:
Charles Champagne Cossette,
Mohammed Ayman Shalaby,
David Saussié,
James Richard Forbes
Abstract:
This paper addresses the problem of decentralized, collaborative state estimation in robotic teams. In particular, this paper considers problems where individual robots estimate similar physical quantities, such as each other's position relative to themselves. The use of pseudomeasurements is introduced as a means of modelling such relationships between robots' state estimates, and is shown to be…
▽ More
This paper addresses the problem of decentralized, collaborative state estimation in robotic teams. In particular, this paper considers problems where individual robots estimate similar physical quantities, such as each other's position relative to themselves. The use of pseudomeasurements is introduced as a means of modelling such relationships between robots' state estimates, and is shown to be a tractable way to approach the decentralized state estimation problem. Moreover, this formulation easily leads to a general-purpose observability test that simultaneously accounts for measurements that robots collect from their own sensors, as well as the communication structure within the team. Finally, input preintegration is proposed as a communication-efficient way of sharing odometry information between robots, and the entire theory is appropriate for both vector-space and Lie-group state definitions. To overcome the need for communicating preintegrated-covariance information, a deep autoencoder is proposed that reconstructs the covariance information from the inputs, hence further reducing the communication requirements. The proposed framework is evaluated on three different simulated problems, and one experiment involving three quadcopters.
△ Less
Submitted 22 January, 2024; v1 submitted 8 April, 2023;
originally announced April 2023.
-
Multi-Robot Relative Pose Estimation and IMU Preintegration Using Passive UWB Transceivers
Authors:
Mohammed Ayman Shalaby,
Charles Champagne Cossette,
Jerome Le Ny,
James Richard Forbes
Abstract:
Ultra-wideband (UWB) systems are becoming increasingly popular as a means of inter-robot ranging and communication. A major constraint associated with UWB is that only one pair of UWB transceivers can range at a time to avoid interference, hence hindering the scalability of UWB-based localization. In this paper, a ranging protocol is proposed that allows all robots to passively listen on neighbour…
▽ More
Ultra-wideband (UWB) systems are becoming increasingly popular as a means of inter-robot ranging and communication. A major constraint associated with UWB is that only one pair of UWB transceivers can range at a time to avoid interference, hence hindering the scalability of UWB-based localization. In this paper, a ranging protocol is proposed that allows all robots to passively listen on neighbouring communicating robots without any hierarchical restrictions on the role of the robots. This is utilized to allow each robot to obtain more range measurements and to broadcast preintegrated inertial measurement unit (IMU) measurements for relative extended pose state estimation directly on SE_2(3). Consequently, a simultaneous clock-synchronization and relative-pose estimator (CSRPE) is formulated using an on-manifold extended Kalman filter (EKF) and is evaluated in simulation using Monte-Carlo runs for up to 7 robots. The ranging protocol is implemented in C on custom-made UWB boards fitted to 3 quadcopters, and the proposed filter is evaluated over multiple experimental trials, yielding up to 56% improvement in localization accuracy.
△ Less
Submitted 11 April, 2023; v1 submitted 7 April, 2023;
originally announced April 2023.
-
Closed-Loop Koopman Operator Approximation
Authors:
Steven Dahdah,
James Richard Forbes
Abstract:
This paper proposes a method to identify a Koopman model of a feedback-controlled system given a known controller. The Koopman operator allows a nonlinear system to be rewritten as an infinite-dimensional linear system by viewing it in terms of an infinite set of lifting functions. A finite-dimensional approximation of the Koopman operator can be identified from data by choosing a finite subset of…
▽ More
This paper proposes a method to identify a Koopman model of a feedback-controlled system given a known controller. The Koopman operator allows a nonlinear system to be rewritten as an infinite-dimensional linear system by viewing it in terms of an infinite set of lifting functions. A finite-dimensional approximation of the Koopman operator can be identified from data by choosing a finite subset of lifting functions and solving a regression problem in the lifted space. Existing methods are designed to identify open-loop systems. However, it is impractical or impossible to run experiments on some systems, such as unstable systems, in an open-loop fashion. The proposed method leverages the linearity of the Koopman operator, along with knowledge of the controller and the structure of the closed-loop system, to simultaneously identify the closed-loop and plant systems. The advantages of the proposed closed-loop Koopman operator approximation method are demonstrated in simulation using a Duffing oscillator and experimentally using a rotary inverted pendulum system. An open-source software implementation of the proposed method is publicly available, along with the experimental dataset generated for this paper.
△ Less
Submitted 1 May, 2024; v1 submitted 27 March, 2023;
originally announced March 2023.
-
Improving Self-Consistency in Underwater Map** Through Laser-Based Loop Closure (Extended)
Authors:
Thomas Hitchcox,
James Richard Forbes
Abstract:
Accurate, self-consistent bathymetric maps are needed to monitor changes in subsea environments and infrastructure. These maps are increasingly collected by underwater vehicles, and map** requires an accurate vehicle navigation solution. Commercial off-the-shelf (COTS) navigation solutions for underwater vehicles often rely on external acoustic sensors for localization, however survey-grade acou…
▽ More
Accurate, self-consistent bathymetric maps are needed to monitor changes in subsea environments and infrastructure. These maps are increasingly collected by underwater vehicles, and map** requires an accurate vehicle navigation solution. Commercial off-the-shelf (COTS) navigation solutions for underwater vehicles often rely on external acoustic sensors for localization, however survey-grade acoustic sensors are expensive to deploy and limit the range of the vehicle. Techniques from the field of simultaneous localization and map**, particularly loop closures, can improve the quality of the navigation solution over dead-reckoning, but are difficult to integrate into COTS navigation systems. This work presents a method to improve the self-consistency of bathymetric maps by smoothly integrating loop-closure measurements into the state estimate produced by a commercial subsea navigation system. Integration is done using a white-noise-on-acceleration motion prior, without access to raw sensor measurements or proprietary models. Improvements in map self-consistency are shown for both simulated and experimental datasets, including a 3D scan of an underwater shipwreck in Wiarton, Ontario, Canada.
△ Less
Submitted 9 June, 2023; v1 submitted 5 January, 2023;
originally announced January 2023.
-
Know What You Don't Know: Consistency in Sliding Window Filtering with Unobservable States Applied to Visual-Inertial SLAM (Extended Version)
Authors:
Daniil Lisus,
Mitchell Cohen,
James Richard Forbes
Abstract:
Estimation algorithms, such as the sliding window filter, produce an estimate and uncertainty of desired states. This task becomes challenging when the problem involves unobservable states. In these situations, it is critical for the algorithm to ``know what it doesn't know'', meaning that it must maintain the unobservable states as unobservable during algorithm deployment. This letter presents ge…
▽ More
Estimation algorithms, such as the sliding window filter, produce an estimate and uncertainty of desired states. This task becomes challenging when the problem involves unobservable states. In these situations, it is critical for the algorithm to ``know what it doesn't know'', meaning that it must maintain the unobservable states as unobservable during algorithm deployment. This letter presents general requirements for maintaining consistency in sliding window filters involving unobservable states. The value of these requirements for designing navigation solutions is experimentally shown within the context of visual-inertial SLAM making use of IMU preintegration.
△ Less
Submitted 8 May, 2023; v1 submitted 13 December, 2022;
originally announced December 2022.
-
Reducing Two-Way Ranging Variance by Signal-Timing Optimization
Authors:
Mohammed Ayman Shalaby,
Charles Champagne Cossette,
James Richard Forbes,
Jerome Le Ny
Abstract:
Time-of-flight-based ranging among transceivers with different clocks requires protocols that accommodate varying rates of the clocks. Double-sided two-way ranging (DS-TWR) is widely adopted as a standard protocol due to its accuracy; however, the precision of DS-TWR has not been clearly addressed. In this paper, an analytical model of the variance of DS-TWR is derived as a function of the user-pr…
▽ More
Time-of-flight-based ranging among transceivers with different clocks requires protocols that accommodate varying rates of the clocks. Double-sided two-way ranging (DS-TWR) is widely adopted as a standard protocol due to its accuracy; however, the precision of DS-TWR has not been clearly addressed. In this paper, an analytical model of the variance of DS-TWR is derived as a function of the user-programmed response delays, which is then compared to the Cramer-Rao Lower Bound (CRLB). This is then used to formulate an optimization problem over the response delays in order to maximize the information gained from range measurements. The derived analytical variance model and optimized protocol are validated experimentally with 2 ranging UWB transceivers, where 29 million range measurements are collected.
△ Less
Submitted 7 February, 2024; v1 submitted 1 November, 2022;
originally announced November 2022.
-
Calibration and Uncertainty Characterization for Ultra-Wideband Two-Way-Ranging Measurements
Authors:
Mohammed Ayman Shalaby,
Charles Champagne Cossette,
James Richard Forbes,
Jerome Le Ny
Abstract:
Ultra-Wideband (UWB) systems are becoming increasingly popular for indoor localization, where range measurements are obtained by measuring the time-of-flight of radio signals. However, the range measurements typically suffer from a systematic error or bias that must be corrected for high-accuracy localization. In this paper, a ranging protocol is proposed alongside a robust and scalable antenna-de…
▽ More
Ultra-Wideband (UWB) systems are becoming increasingly popular for indoor localization, where range measurements are obtained by measuring the time-of-flight of radio signals. However, the range measurements typically suffer from a systematic error or bias that must be corrected for high-accuracy localization. In this paper, a ranging protocol is proposed alongside a robust and scalable antenna-delay calibration procedure to accurately and efficiently calibrate antenna delays for many UWB tags. Additionally, the bias and uncertainty of the measurements are modelled as a function of the received-signal power. The full calibration procedure is presented using experimental training data of 3 aerial robots fitted with 2 UWB tags each, and then evaluated on 2 test experiments. A localization problem is then formulated on the experimental test data, and the calibrated measurements and their modelled uncertainty are fed into an extended Kalman filter (EKF). The proposed calibration is shown to yield an average of 46% improvement in localization accuracy. Lastly, the paper is accompanied by an open-source UWB-calibration Python library, which can be found at https://github.com/decargroup/uwb_calibration.
△ Less
Submitted 16 February, 2023; v1 submitted 11 October, 2022;
originally announced October 2022.
-
Performance Evaluation of 3D Keypoint Detectors and Descriptors on Coloured Point Clouds in Subsea Environments
Authors:
Kyungmin Jung,
Thomas Hitchcox,
James Richard Forbes
Abstract:
The recent development of high-precision subsea optical scanners allows for 3D keypoint detectors and feature descriptors to be leveraged on point cloud scans from subsea environments. However, the literature lacks a comprehensive survey to identify the best combination of detectors and descriptors to be used in these challenging and novel environments. This paper aims to identify the best detecto…
▽ More
The recent development of high-precision subsea optical scanners allows for 3D keypoint detectors and feature descriptors to be leveraged on point cloud scans from subsea environments. However, the literature lacks a comprehensive survey to identify the best combination of detectors and descriptors to be used in these challenging and novel environments. This paper aims to identify the best detector/descriptor pair using a challenging field dataset collected using a commercial underwater laser scanner. Furthermore, studies have shown that incorporating texture information to extend geometric features adds robustness to feature matching on synthetic datasets. This paper also proposes a novel method of fusing images with underwater laser scans to produce coloured point clouds, which are used to study the effectiveness of 6D point cloud descriptors.
△ Less
Submitted 26 February, 2024; v1 submitted 26 September, 2022;
originally announced September 2022.
-
Relative Position Estimation in Multi-Agent Systems Using Attitude-Coupled Range Measurements
Authors:
Mohammed Shalaby,
Charles Champagne Cossette,
James Richard Forbes,
Jerome Le Ny
Abstract:
The ability to accurately estimate the position of robotic agents relative to one another, in possibly GPS-denied environments, is crucial to execute collaborative tasks. Inter-agent range measurements are available at a low cost, due to technologies such as ultra-wideband radio. However, the task of three-dimensional relative position estimation using range measurements in multi-agent systems suf…
▽ More
The ability to accurately estimate the position of robotic agents relative to one another, in possibly GPS-denied environments, is crucial to execute collaborative tasks. Inter-agent range measurements are available at a low cost, due to technologies such as ultra-wideband radio. However, the task of three-dimensional relative position estimation using range measurements in multi-agent systems suffers from unobservabilities. This letter presents a sufficient condition for the observability of the relative positions, and satisfies the condition using a simple framework with only range measurements, an accelerometer, a rate gyro, and a magnetometer. The framework has been tested in simulation and in experiments, where 40-50 cm positioning accuracy is achieved using inexpensive off-the-shelf hardware.
△ Less
Submitted 16 July, 2022;
originally announced July 2022.
-
Mind the Gap: Norm-Aware Adaptive Robust Loss for Multivariate Least-Squares Problems
Authors:
Thomas Hitchcox,
James Richard Forbes
Abstract:
Measurement outliers are unavoidable when solving real-world robot state estimation problems. A large family of robust loss functions (RLFs) exists to mitigate the effects of outliers, including newly developed adaptive methods that do not require parameter tuning. All of these methods assume that residuals follow a zero-mean Gaussian-like distribution. However, in multivariate problems the residu…
▽ More
Measurement outliers are unavoidable when solving real-world robot state estimation problems. A large family of robust loss functions (RLFs) exists to mitigate the effects of outliers, including newly developed adaptive methods that do not require parameter tuning. All of these methods assume that residuals follow a zero-mean Gaussian-like distribution. However, in multivariate problems the residual is often defined as a norm, and norms follow a Chi-like distribution with a non-zero mode value. This produces a "mode gap" that impacts the convergence rate and accuracy of existing RLFs. The proposed approach, "Adaptive MB," accounts for this gap by first estimating the mode of the residuals using an adaptive Chi-like distribution. Applying an existing adaptive weighting scheme only to residuals greater than the mode leads to more robust performance and faster convergence times in two fundamental state estimation problems, point cloud alignment and pose averaging.
△ Less
Submitted 10 June, 2023; v1 submitted 18 June, 2022;
originally announced June 2022.
-
Optimal Multi-robot Formations for Relative Pose Estimation Using Range Measurements
Authors:
Charles Champagne Cossette,
Mohammed Ayman Shalaby,
David Saussie,
Jerome Le Ny,
James Richard Forbes
Abstract:
In multi-robot missions, relative position and attitude information between agents is valuable for a variety of tasks such as map**, planning, and formation control. In this paper, the problem of estimating relative poses from a set of inter-agent range measurements is investigated. Specifically, it is shown that the estimation accuracy is highly dependent on the true relative poses themselves,…
▽ More
In multi-robot missions, relative position and attitude information between agents is valuable for a variety of tasks such as map**, planning, and formation control. In this paper, the problem of estimating relative poses from a set of inter-agent range measurements is investigated. Specifically, it is shown that the estimation accuracy is highly dependent on the true relative poses themselves, which prompts the desire to find multi-agent formations that provide the best estimation performance. By direct maximization of Fischer information, it is shown in simulation and experiment that large improvements in estimation accuracy can be obtained by optimizing the formation geometry of a team of robots.
△ Less
Submitted 27 May, 2022;
originally announced May 2022.
-
Ultra-Wideband Teach and Repeat
Authors:
Mohammed Ayman Shalaby,
Charles Champagne Cossette,
Jerome Le Ny,
James Richard Forbes
Abstract:
Autonomously retracing a manually-taught path is desirable for many applications, and Teach and Repeat (T&R) algorithms present an approach that is suitable for long-range autonomy. In this paper, ultra-wideband (UWB) ranging-based T&R is proposed for vehicles with limited resources. By fixing single UWB transceivers at far-apart unknown locations in an indoor environment, a robot with 3 UWB trans…
▽ More
Autonomously retracing a manually-taught path is desirable for many applications, and Teach and Repeat (T&R) algorithms present an approach that is suitable for long-range autonomy. In this paper, ultra-wideband (UWB) ranging-based T&R is proposed for vehicles with limited resources. By fixing single UWB transceivers at far-apart unknown locations in an indoor environment, a robot with 3 UWB transceivers builds a locally consistent map during the teach pass by fusing the range measurements under a custom ranging protocol with an on-board IMU and height measurements. The robot then uses information from the teach pass to retrace the same trajectory autonomously. The proposed ranging protocol and T&R algorithm are validated in simulation, where it is shown that the robot can successfully retrace the taught trajectory with sub-metre tracking error.
△ Less
Submitted 2 February, 2022;
originally announced February 2022.
-
System Norm Regularization Methods for Koopman Operator Approximation
Authors:
Steven Dahdah,
James Richard Forbes
Abstract:
Approximating the Koopman operator from data is numerically challenging when many lifting functions are considered. Even low-dimensional systems can yield unstable or ill-conditioned results in a high-dimensional lifted space. In this paper, Extended Dynamic Mode Decomposition (DMD) and DMD with control, two methods for approximating the Koopman operator, are reformulated as convex optimization pr…
▽ More
Approximating the Koopman operator from data is numerically challenging when many lifting functions are considered. Even low-dimensional systems can yield unstable or ill-conditioned results in a high-dimensional lifted space. In this paper, Extended Dynamic Mode Decomposition (DMD) and DMD with control, two methods for approximating the Koopman operator, are reformulated as convex optimization problems with linear matrix inequality constraints. Asymptotic stability constraints and system norm regularizers are then incorporated as methods to improve the numerical conditioning of the Koopman operator. Specifically, the H-infinity norm is used to penalize the input-output gain of the Koopman system. Weighting functions are then applied to penalize the system gain at specific frequencies. These constraints and regularizers introduce bilinear matrix inequality constraints to the regression problem, which are handled by solving a sequence of convex optimization problems. Experimental results using data from an aircraft fatigue structural test rig and a soft robot arm highlight the advantages of the proposed regression methods.
△ Less
Submitted 9 August, 2022; v1 submitted 18 October, 2021;
originally announced October 2021.
-
Localization with Directional Coordinates
Authors:
Charles Champagne Cossette,
Mohammed Shalaby,
David Saussié,
James Richard Forbes
Abstract:
A coordinate system is proposed that replaces the usual three-dimensional Cartesian x,y,z position coordinates, for use in robotic localization applications. Range, azimuth, and elevation measurement models become greatly simplified, and, unlike spherical coordinates, the proposed coordinates do not suffer from the same kinematic singularities and angle wrap-around. When compared to Cartesian coor…
▽ More
A coordinate system is proposed that replaces the usual three-dimensional Cartesian x,y,z position coordinates, for use in robotic localization applications. Range, azimuth, and elevation measurement models become greatly simplified, and, unlike spherical coordinates, the proposed coordinates do not suffer from the same kinematic singularities and angle wrap-around. When compared to Cartesian coordinates, the proposed coordinate system results in a significantly enhanced ability to represent the true distribution of robot positions, ultimately leading to large improvements in state estimation consistency.
△ Less
Submitted 19 September, 2021;
originally announced September 2021.
-
Koopman Linearization for Data-Driven Batch State Estimation of Control-Affine Systems
Authors:
Zi Cong Guo,
Vassili Korotkine,
James R. Forbes,
Timothy D. Barfoot
Abstract:
We present the Koopman State Estimator (KoopSE), a framework for model-free batch state estimation of control-affine systems that makes no linearization assumptions, requires no problem-specific feature selections, and has an inference computational cost that is independent of the number of training points. We lift the original nonlinear system into a higher-dimensional Reproducing Kernel Hilbert…
▽ More
We present the Koopman State Estimator (KoopSE), a framework for model-free batch state estimation of control-affine systems that makes no linearization assumptions, requires no problem-specific feature selections, and has an inference computational cost that is independent of the number of training points. We lift the original nonlinear system into a higher-dimensional Reproducing Kernel Hilbert Space (RKHS), where the system becomes bilinear. The time-invariant model matrices can be learned by solving a least-squares problem on training trajectories. At test time, the system is algebraically manipulated into a linear time-varying system, where standard batch linear state estimation techniques can be used to efficiently compute state means and covariances. Random Fourier Features (RFF) are used to combine the computational efficiency of Koopman-based methods and the generality of kernel-embedding methods. KoopSE is validated experimentally on a localization task involving a mobile robot equipped with ultra-wideband receivers and wheel odometry. KoopSE estimates are more accurate and consistent than the standard model-based extended Rauch-Tung-Striebel (RTS) smoother, despite KoopSE having no prior knowledge of the system's motion or measurement models.
△ Less
Submitted 3 December, 2021; v1 submitted 14 September, 2021;
originally announced September 2021.
-
Heading Estimation Using Ultra-Wideband Received Signal Strength and Gaussian Processes
Authors:
Daniil Lisus,
Charles Champagne Cossette,
Mohammed Shalaby,
James Richard Forbes
Abstract:
It is essential that a robot has the ability to determine its position and orientation to execute tasks autonomously. Heading estimation is especially challenging in indoor environments where magnetic distortions make magnetometer-based heading estimation difficult. Ultra-wideband (UWB) transceivers are common in indoor localization problems. This letter experimentally demonstrates how to use UWB…
▽ More
It is essential that a robot has the ability to determine its position and orientation to execute tasks autonomously. Heading estimation is especially challenging in indoor environments where magnetic distortions make magnetometer-based heading estimation difficult. Ultra-wideband (UWB) transceivers are common in indoor localization problems. This letter experimentally demonstrates how to use UWB range and received signal strength (RSS) measurements to estimate robot heading. The RSS of a UWB antenna varies with its orientation. As such, a Gaussian process (GP) is used to learn a data-driven relationship from UWB range and RSS inputs to orientation outputs. Combined with a gyroscope in an invariant extended Kalman filter, this realizes a heading estimation method that uses only UWB and gyroscope measurements.
△ Less
Submitted 10 September, 2021;
originally announced September 2021.
-
Finite-Horizon LQR Control of Quadrotors on $SE_2(3)$
Authors:
Mitchell R. Cohen,
Khairi Abdulrahim,
James Richard Forbes
Abstract:
This paper considers optimal control of a quadrotor unmanned aerial vehicles (UAV) using the discrete-time, finite-horizon, linear quadratic regulator (LQR). The state of a quadrotor UAV is represented as an element of the matrix Lie group of double direct isometries, $SE_2(3)$. The nonlinear system is linearized using a left-invariant error about a reference trajectory, leading to an optimal gain…
▽ More
This paper considers optimal control of a quadrotor unmanned aerial vehicles (UAV) using the discrete-time, finite-horizon, linear quadratic regulator (LQR). The state of a quadrotor UAV is represented as an element of the matrix Lie group of double direct isometries, $SE_2(3)$. The nonlinear system is linearized using a left-invariant error about a reference trajectory, leading to an optimal gain sequence that can be calculated offline. The reference trajectory is calculated using the differentially flat properties of the quadrotor. Monte-Carlo simulations demonstrate robustness of the proposed control scheme to parametric uncertainty, state-estimation error, and initial error. Additionally, when compared to an LQR controller that uses a conventional error definition, the proposed controller demonstrates better performance when initial errors are large.
△ Less
Submitted 28 May, 2021;
originally announced May 2021.
-
The Complex-Step Derivative Approximation on Matrix Lie Groups
Authors:
Charles Champagne Cossette,
Alex Walsh,
James Richard Forbes
Abstract:
The complex-step derivative approximation is a numerical differentiation technique that can achieve analytical accuracy, to machine precision, with a single function evaluation. In this letter, the complex-step derivative approximation is extended to be compatible with elements of matrix Lie groups. As with the standard complex-step derivative, the method is still able to achieve analytical accura…
▽ More
The complex-step derivative approximation is a numerical differentiation technique that can achieve analytical accuracy, to machine precision, with a single function evaluation. In this letter, the complex-step derivative approximation is extended to be compatible with elements of matrix Lie groups. As with the standard complex-step derivative, the method is still able to achieve analytical accuracy, up to machine precision, with a single function evaluation. Compared to a central-difference scheme, the proposed complex-step approach is shown to have superior accuracy. The approach is applied to two different pose estimation problems, and is able to recover the same results as an analytical method when available.
△ Less
Submitted 6 May, 2021;
originally announced May 2021.
-
Invariant Extended Kalman Filtering Using Two Position Receivers for Extended Pose Estimation
Authors:
Natalia Pavlasek,
Alex Walsh,
James Richard Forbes
Abstract:
This paper considers the use of two position receivers and an inertial measurement unit (IMU) to estimate the position, velocity, and attitude of a rigid body, collectively called extended pose. The measurement model consisting of the position of one receiver and the relative position between the two receivers is left invariant, enabling the use of the invariant extended Kalman filter (IEKF) frame…
▽ More
This paper considers the use of two position receivers and an inertial measurement unit (IMU) to estimate the position, velocity, and attitude of a rigid body, collectively called extended pose. The measurement model consisting of the position of one receiver and the relative position between the two receivers is left invariant, enabling the use of the invariant extended Kalman filter (IEKF) framework. The IEKF possesses various advantages over the standard multiplicative extended Kalman filter, such as state-estimate-independent Jacobians. Monte Carlo simulations demonstrate that the two-receiver IEKF approach yields improved estimates over a two-receiver multiplicative extended Kalman filter (MEKF) and a single-receiver IEKF approach. An experiment further validates the proposed approach, confirming that the two-receiver IEKF has improved performance over the other filters considered.
△ Less
Submitted 29 April, 2021;
originally announced April 2021.
-
Relative Position Estimation Between Two UWB Devices with IMUs
Authors:
Charles C. Cossette,
Mohammed Shalaby,
David Saussié,
James R. Forbes,
Jérôme Le Ny
Abstract:
For a team of robots to work collaboratively, it is crucial that each robot have the ability to determine the position of their neighbors, relative to themselves, in order to execute tasks autonomously. This letter presents an algorithm for determining the three-dimensional relative position between two mobile robots, each using nothing more than a single ultra-wideband transceiver, an acceleromet…
▽ More
For a team of robots to work collaboratively, it is crucial that each robot have the ability to determine the position of their neighbors, relative to themselves, in order to execute tasks autonomously. This letter presents an algorithm for determining the three-dimensional relative position between two mobile robots, each using nothing more than a single ultra-wideband transceiver, an accelerometer, a rate gyro, and a magnetometer. A sliding window filter estimates the relative position at selected keypoints by combining the distance measurements with acceleration estimates, which each agent computes using an on-board attitude estimator. The algorithm is appropriate for real-time implementation, and has been tested in simulation and experiment, where it comfortably outperforms standard estimators. A positioning accuracy of less than 1 meter is achieved with inexpensive sensors.
△ Less
Submitted 21 April, 2021;
originally announced April 2021.
-
Vectorial Parameterizations of Pose
Authors:
Timothy D. Barfoot,
James R. Forbes,
Gabriele M. T. D'Eleuterio
Abstract:
Robotics and computer vision problems commonly require handling rigid-body motions comprising translation and rotation - together referred to as pose. In some situations, a vectorial parameterization of pose can be useful, where elements of a vector space are surjectively mapped to a matrix Lie group. For example, these vectorial representations can be employed for optimization as well as uncertai…
▽ More
Robotics and computer vision problems commonly require handling rigid-body motions comprising translation and rotation - together referred to as pose. In some situations, a vectorial parameterization of pose can be useful, where elements of a vector space are surjectively mapped to a matrix Lie group. For example, these vectorial representations can be employed for optimization as well as uncertainty representation on groups. The most common map** is the matrix exponential, which maps elements of a Lie algebra onto the associated Lie group. However, this choice is not unique. It has been previously shown how to characterize all such vectorial parameterizations for SO(3), the group of rotations. Some results are also known for the group of poses, where it is possible to build a family of vectorial map**s that includes the matrix exponential as well as the Cayley transformation. We extend what is known for these pose map**s to the 4 x 4 representation common in robotics, and also demonstrate three different examples of the proposed pose map**s: (i) pose interpolation, (ii) pose servoing control, and (iii) pose estimation in a pointcloud alignment problem. In the pointcloud alignment problem our results lead to a new algorithm based on the Cayley transformation, which we call CayPer.
△ Less
Submitted 4 December, 2021; v1 submitted 12 March, 2021;
originally announced March 2021.
-
Cascaded Filtering Using the Sigma Point Transformation (Extended Version)
Authors:
Mohammed Shalaby,
Charles Champagne Cossette,
Jerome Le Ny,
James Richard Forbes
Abstract:
It is often convenient to separate a state estimation task into smaller "local" tasks, where each local estimator estimates a subset of the overall system state. However, neglecting cross-covariance terms between state estimates can result in overconfident estimates, which can ultimately degrade the accuracy of the estimator. Common cascaded filtering techniques focus on the problem of modelling c…
▽ More
It is often convenient to separate a state estimation task into smaller "local" tasks, where each local estimator estimates a subset of the overall system state. However, neglecting cross-covariance terms between state estimates can result in overconfident estimates, which can ultimately degrade the accuracy of the estimator. Common cascaded filtering techniques focus on the problem of modelling cross-covariances when the local estimators share a common state vector. This letter introduces a novel cascaded and decentralized filtering approach that approximates the cross-covariances when the local estimators consider distinct state vectors. The proposed estimator is validated in simulations and in experiments on a three-dimensional attitude and position estimation problem. The proposed approach is compared to a naive cascaded filtering approach that neglects cross-covariance terms, a sigma point-based Covariance Intersection filter, and a full-state filter. In both simulations and experiments, the proposed filter outperforms the naive and the Covariance Intersection filters, while performing comparatively to the full-state filter.
△ Less
Submitted 6 March, 2021;
originally announced March 2021.
-
Linear Matrix Inequality Approaches to Koopman Operator Approximation
Authors:
Steven Dahdah,
James Richard Forbes
Abstract:
The regression problem associated with finding a matrix approximation of the Koopman operator from data is considered. The regression problem is formulated as a convex optimization problem subject to linear matrix inequality (LMI) constraints. Doing so allows for additional LMI constraints to be incorporated into the regression problem. In particular, asymptotic stability constraints, regularizati…
▽ More
The regression problem associated with finding a matrix approximation of the Koopman operator from data is considered. The regression problem is formulated as a convex optimization problem subject to linear matrix inequality (LMI) constraints. Doing so allows for additional LMI constraints to be incorporated into the regression problem. In particular, asymptotic stability constraints, regularization using matrix norms, and even regularization using system norms can be easily incorporated into the regression problem.
△ Less
Submitted 18 October, 2021; v1 submitted 6 February, 2021;
originally announced February 2021.
-
Exactly Sparse Gaussian Variational Inference with Application to Derivative-Free Batch Nonlinear State Estimation
Authors:
Timothy D. Barfoot,
James R. Forbes,
David Yoon
Abstract:
We present a Gaussian Variational Inference (GVI) technique that can be applied to large-scale nonlinear batch state estimation problems. The main contribution is to show how to fit both the mean and (inverse) covariance of a Gaussian to the posterior efficiently, by exploiting factorization of the joint likelihood of the state and data, as is common in practical problems. This is different than M…
▽ More
We present a Gaussian Variational Inference (GVI) technique that can be applied to large-scale nonlinear batch state estimation problems. The main contribution is to show how to fit both the mean and (inverse) covariance of a Gaussian to the posterior efficiently, by exploiting factorization of the joint likelihood of the state and data, as is common in practical problems. This is different than Maximum A Posteriori (MAP) estimation, which seeks the point estimate for the state that maximizes the posterior (i.e., the mode). The proposed Exactly Sparse Gaussian Variational Inference (ESGVI) technique stores the inverse covariance matrix, which is typically very sparse (e.g., block-tridiagonal for classic state estimation). We show that the only blocks of the (dense) covariance matrix that are required during the calculations correspond to the non-zero blocks of the inverse covariance matrix, and further show how to calculate these blocks efficiently in the general GVI problem. ESGVI operates iteratively, and while we can use analytical derivatives at each iteration, Gaussian cubature can be substituted, thereby producing an efficient derivative-free batch formulation. ESGVI simplifies to precisely the Rauch-Tung-Striebel (RTS) smoother in the batch linear estimation case, but goes beyond the 'extended' RTS smoother in the nonlinear case since it finds the best-fit Gaussian (mean and covariance), not the MAP point estimate. We demonstrate the technique on controlled simulation problems and a batch nonlinear Simultaneous Localization and Map** (SLAM) problem with an experimental dataset.
△ Less
Submitted 9 April, 2020; v1 submitted 9 November, 2019;
originally announced November 2019.
-
Differential Geometric SLAM
Authors:
David Evan Zlotnik,
James Richard Forbes
Abstract:
The simultaneous localization and map** (SLAM) problem is considered in three dimensions. The proposed algorithm, differential geometric SLAM (DG-SLAM), employs methods from differential geometry to propagate the state and map estimates. Unlike EKF SLAM, the proposed filter is provably asymptotically stable under the assumption of no measurement noise or biases. The robustness of the DG-SLAM alg…
▽ More
The simultaneous localization and map** (SLAM) problem is considered in three dimensions. The proposed algorithm, differential geometric SLAM (DG-SLAM), employs methods from differential geometry to propagate the state and map estimates. Unlike EKF SLAM, the proposed filter is provably asymptotically stable under the assumption of no measurement noise or biases. The robustness of the DG-SLAM algorithm is assessed in simulation with measurement noise. The simulation demonstrates successful localization and map**.
△ Less
Submitted 1 June, 2015;
originally announced June 2015.
