Relative Pose Estimation using Range-only Measurements with Large Initial Uncertainty
01 Jun 2018-pp 5055-5061
TL;DR: This formulation provides a lightweight and robust relative initialization approach that identifies an accurate relative pose and would be appropriate for relative guidance, initialization of more complex (multi-vehicle) navigation approaches, or shared, synthetic aperture-like measurement processing.
Abstract: In this paper, a relative frame localization problem is addressed for pairs of vehicles with significant initial pose uncertainties. A relative frame Extended Kalman filter (EKF) is developed for the case where vehicles share odometry (body-frame delta position values), and are capable of getting inter-vehicle range measurements. The extended Kalman filter is not robust to large initial errors in this nonlinear system, therefore a Multi-Hypothesis approach is used to accommodate the unknown, up to approximate range, initial relative pose (relative position and orientation). This formulation provides a lightweight and robust relative initialization approach that identifies an accurate relative pose and would be appropriate for relative guidance, initialization of more complex (multi-vehicle) navigation approaches, or shared, synthetic aperture-like measurement processing. Results are presented for both simulated and hardware implementations of the filter.
Citations
More filters
TL;DR: A framework that allows estimation of the relative pose and orientation between vehicles in the presence of high initial uncertainty and Improvements in the system performance when cooperation is introduced among vehicles are highlighted through experiments.
Abstract: The ability for autonomous vehicles to cooperatively navigate, especially in GPS denied environments, is becoming increasingly important. It also requires the ability to initialize, or reinitialize estimation algorithms for cooperative systems on-the-fly in cases where precise a priori state information is unavailable. In this paper, we provide a framework that allows estimation of the relative pose and orientation between vehicles in the presence of high initial uncertainty. Effects of cooperation among multiple vehicles exchanging estimates of heading rate and velocity and external sensor measurements are analyzed. A Multi-Hypothesis Extended Kalman Filter (MHEKF) technique is used to initialize pairwise vehicles using range-only measurements. Using solutions identified by the MHEKF algorithm, a joint filter comprising of multiple vehicles is initialized. Sufficient conditions to maintain bounded errors are derived through nonlinear observability analysis using Lie derivatives for the pairwise and the multi-vehicle cases. Using these conditions as passive constraints in the system, simulation and hardware experiments are performed to demonstrate the advantages of using MHEKF when the initial conditions are unreliable. A multi-vehicle testbed for heterogeneous platforms with different sensing modalities is developed to facilitate hardware testing. Improvements in the system performance when cooperation is introduced among vehicles is also highlighted through experiments.
9 citations
Cites background or methods from "Relative Pose Estimation using Rang..."
...In this paper, we extend the 2-vehicle relative localization work [33] to an N-vehicle cooperative relative localization problem using the concept of MHEKF to initialize an N-vehicle joint filter with no a-priori information (no position or heading information is present)....
[...]
...The MHEKF proposed in [33] and further improved upon in this paper is a technique developed to address the issue of initialization of estimation algorithms in the presence of large initial uncertainty or low/none a-priori information, especially for platforms with low processing power and real-time implementation....
[...]
...• Improvement of the Multi-Hypothesis Extended Kalman Filter (MHEKF) technique introduced in [33] by removing a-priori heading information....
[...]
...The initialization problem for the relative position was solved using a-priori noisy heading information in [33]....
[...]
...In [33], the authors use noisy magnetometer readings to initialize the relative heading between pairwise vehicles....
[...]
Proceedings Article•
01 Jul 2019TL;DR: The Square Root Partial-Update Kalman Filter is developed and demonstrated and benefits from both the numerical stability inherent in the square root filter and the robustness of the partial update while operating directly on thesquare root representation of the uncertainty.
Abstract: The Partial-Update Kalman Filter is a recent development that, in a very simple fashion, extends the uncertainty and nonlinearities which the extended or unscented Kalman filters are able to tolerate. However, to date its application has been limited to filters using a full covariance matrix representations or through a wasteful mapping between square root and full covariance representations. In this paper, the Square Root Partial-Update Kalman Filter is developed and demonstrated. This new approach benefits from both the numerical stability inherent in the square root filter and the robustness of the partial update while operating directly on the square root representation of the uncertainty. The paper details the algorithm and implementation and provides a nonlinear filtering example to demonstrate the increased robustness of the partial update form of the square root filter.
5 citations
Cites methods from "Relative Pose Estimation using Rang..."
...Examples of successful implementations of the Partial-Update Kalman filter can be found in [17], [18] and [19]....
[...]
01 Dec 2018
TL;DR: Using nonlinear observability analysis, it is shown that the combined algorithm proposed in this paper keeps the states of target and UVs observable as long as the observability criteria are not violated.
Abstract: In this paper, we address the problem of synchronized convergence of multiple Unmanned Vehicles (UVs) onto a single moving and maneuvering target in a GPS-denied environment with each agent having fractionated information and sensing constraints. The UVs (or agents) are considered to be distributed over a topographical region with limited sensing range and field-of-view (FOV) as compared to the expanse of the region under consideration. These constraints are addressed using cooperative localization technique augmented with Proportional Navigation (PN) guidance law. Using nonlinear observability analysis, it is shown that the combined algorithm proposed in this paper keeps the states of target and UVs observable as long as the observability criteria are not violated. Further, simulations are performed to show that the theory is consistent and is able to perform simultaneous convergence.
3 citations
Cites methods from "Relative Pose Estimation using Rang..."
...We estimate the velocity and heading of the target using the concepts discussed in [9] and [10]....
[...]
...VT and ψT are estimated using [9] and [10] as discussed in Section I....
[...]
01 Jan 2019
TL;DR: A method to estimate the relative position and relative heading of an Unmanned Aerial Vehicle attempting to land on a moving platform using range-only measurements under GPS-denied conditions is developed.
Abstract: In this paper, we develop a method to estimate the relative position and relative heading of an Unmanned Aerial Vehicle (UAV) attempting to land on a moving platform using range-only measurements under GPS-denied conditions. Landing problems typically require precise estimation of relative position and heading, the proposed solution estimates relative pose between the platform (in this case a ship deck) and the landing multirotor. Vision-based landing techniques although accurate will fail under hostile weather conditions with the lack of Global Positioning System (GPS). This formulation investigates the effect on estimation quality when a single vehicle attempts to land using measurements from the ship only versus a team of multirotors assisting a vehicle to land on the ship. A multi-vehicle simulator has been developed in MATLAB/Simulink that facilitates testing our approaches. The simulation results clearly demonstrate the advantage of using a team for assisted landing rather than alone vehicle performing relative pose estimation using a limited range measurements.
3 citations
01 Jan 2018
TL;DR: The goal of this work is to show that many, small, potentially-lower-cost vehicles could collaboratively localize better than a single, more-accurate, higher-cost GPS-denied system.
Abstract: This paper describes a vision and proposes a method for multiple, small, fixed-wing aircraft cooperatively localizing in GPS-denied environments. Recent work has focused on the development of a monocular, visualinertial odometry for fixed-wing aircraft that accounts for fixed-wing flight characteristics and sensing requirements. The odometry was developed to be a front-end for novel methodology called relative navigation, which has been developed in prior work. This paper describes how the front-end could enable a back-end where odometry from multiple vehicles and inter-vehicle measurements could be used in a single, global, back-end, graph-based optimization. The inter-vehicle measurements over constrain the graph and allow the optimization to remove accumulated drift for more accurate estimates. The goal of this work is to show that many, small, potentially-lower-cost vehicles could collaboratively localize better than a single, more-accurate, higher-cost GPS-denied system.
1 citations
Cites background from "Relative Pose Estimation using Rang..."
...The work in [23] also directly deals with large initial uncertainty for optimizing graphs with inter-vehicle range measurements....
[...]
References
More filters
28 Jul 1997
TL;DR: It is argued that the ease of implementation and more accurate estimation features of the new filter recommend its use over the EKF in virtually all applications.
Abstract: The Kalman Filter (KF) is one of the most widely used methods for tracking and estimation due to its simplicity, optimality, tractability and robustness. However, the application of the KF to nonlinear systems can be difficult. The most common approach is to use the Extended Kalman Filter (EKF) which simply linearizes all nonlinear models so that the traditional linear Kalman filter can be applied. Although the EKF (in its many forms) is a widely used filtering strategy, over thirty years of experience with it has led to a general consensus within the tracking and control community that it is difficult to implement, difficult to tune, and only reliable for systems which are almost linear on the time scale of the update intervals. In this paper a new linear estimator is developed and demonstrated. Using the principle that a set of discretely sampled points can be used to parameterize mean and covariance, the estimator yields performance equivalent to the KF for linear systems yet generalizes elegantly to nonlinear systems without the linearization steps required by the EKF. We show analytically that the expected performance of the new approach is superior to that of the EKF and, in fact, is directly comparable to that of the second order Gauss filter. The method is not restricted to assuming that the distributions of noise sources are Gaussian. We argue that the ease of implementation and more accurate estimation features of the new filter recommend its use over the EKF in virtually all applications.
5,314 citations
"Relative Pose Estimation using Rang..." refers methods in this paper
...The concept of the partial update is similar to a Schmidt-Kalman filter [20], but instead of holding problematic states/parameters fixed, it allows for those states to receive partial updates....
[...]
01 Jun 1991
TL;DR: An algorithm for, model-based localization that relies on the concept of a geometric beacon, a naturally occurring environment feature that can be reliably observed in successive sensor measurements and can be accurately described in terms of a concise geometric parameterization, is developed.
Abstract: The application of the extended Kaman filter to the problem of mobile robot navigation in a known environment is presented. An algorithm for, model-based localization that relies on the concept of a geometric beacon, a naturally occurring environment feature that can be reliably observed in successive sensor measurements and can be accurately described in terms of a concise geometric parameterization, is developed. The algorithm is based on an extended Kalman filter that utilizes matches between observed geometric beacons and an a priori map of beacon locations. Two implementations of this navigation algorithm, both of which use sonar, are described. The first implementation uses a simple vehicle with point kinematics equipped with a single rotating sonar. The second implementation uses a 'Robuter' mobile robot and six static sonar transducers to provide localization information while the vehicle moves at typical speeds of 30 cm/s. >
1,394 citations
"Relative Pose Estimation using Rang..." refers methods in this paper
...And relative localization has been used for swarm control [10], target tracking [11] and target localization [12] applications....
[...]
03 Nov 2004
TL;DR: A distributed, linear-time algorithm for localizing sensor network nodes in the presence of range measurement noise is described and the probabilistic notion of robust quadrilaterals is introduced as a way to avoid flip ambiguities that otherwise corrupt localization computations.
Abstract: This paper describes a distributed, linear-time algorithm for localizing sensor network nodes in the presence of range measurement noise and demonstrates the algorithm on a physical network. We introduce the probabilistic notion of robust quadrilaterals as a way to avoid flip ambiguities that otherwise corrupt localization computations. We formulate the localization problem as a two-dimensional graph realization problem: given a planar graph with approximately known edge lengths, recover the Euclidean position of each vertex up to a global rotation and translation. This formulation is applicable to the localization of sensor networks in which each node can estimate the distance to each of its neighbors, but no absolute position reference such as GPS or fixed anchor nodes is available.We implemented the algorithm on a physical sensor network and empirically assessed its accuracy and performance. Also, in simulation, we demonstrate that the algorithm scales to large networks and handles real-world deployment geometries. Finally, we show how the algorithm supports localization of mobile nodes.
1,086 citations
"Relative Pose Estimation using Rang..." refers methods in this paper
...The relative position estimation problem has been solved using various techniques like EKF in both centralized form [8] and distributed form [9]....
[...]
01 Sep 2012
TL;DR: This document provides a hands-on introduction to both factor graphs and GTSAM, a BSD-licensed C++ library based on factor graphs developed at the Georgia Institute of Technology by myself, many of my students, and collaborators.
Abstract: In this document I provide a hands-on introduction to both factor graphs and GTSAM. Factor graphs are graphical models (Koller and Friedman, 2009) that are well suited to modeling complex estimation problems, such as Simultaneous Localization and Mapping (SLAM) or Structure from Motion (SFM). You might be familiar with another often used graphical model, Bayes networks, which are directed acyclic graphs. A factor graph, however, is a bipartite graph consisting of factors connected to variables. The variables represent the unknown random variables in the estimation problem, whereas the factors represent probabilistic information on those variables, derived from measurements or prior knowledge. In the following sections I will show many examples from both robotics and vision. The GTSAM toolbox (GTSAM stands for “Georgia Tech Smoothing and Mapping”) toolbox is a BSD-licensed C++ library based on factor graphs, developed at the Georgia Institute of Technology by myself, many of my students, and collaborators. It provides state of the art solutions to the SLAM and SFM problems, but can also be used to model and solve both simpler and more complex estimation problems. It also provides a MATLAB interface which allows for rapid prototype development, visualization, and user interaction. GTSAM exploits sparsity to be computationally efficient. Typically measurements only provide information on the relationship between a handful of variables, and hence the resulting factor graph will be sparsely connected. This is exploited by the algorithms implemented in GTSAM to reduce computational complexity. Even when graphs are too dense to be handled efficiently by direct methods, GTSAM provides iterative methods that are quite efficient regardless. You can download the latest version of GTSAM at http://tinyurl.com/gtsam.
563 citations
"Relative Pose Estimation using Rang..." refers methods in this paper
...The assumption of good communication is limiting in practice, however, if EKF implementations are found to be effective under these assumptions then it is reasonable to believe that data dropouts can be handled using batch processing, such as a modified GTSAM [16] or G20 [17] if necessary....
[...]
TL;DR: A virtual force algorithm (VFA) is proposed as a sensor deployment strategy to enhance the coverage after an initial random placement of sensors to improve the coverage of cluster-based distributed sensor networks.
Abstract: The effectiveness of cluster-based distributed sensor networks depends to a large extent on the coverage provided by the sensor deployment. We propose a virtual force algorithm (VFA) as a sensor deployment strategy to enhance the coverage after an initial random placement of sensors. For a given number of sensors, the VFA algorithm attempts to maximize the sensor field coverage. A judicious combination of attractive and repulsive forces is used to determine the new sensor locations that improve the coverage. Once the effective sensor positions are identified, a one-time movement with energy consideration incorporated is carried out, that is, the sensors are redeployed, to these positions. We also propose a novel probabilistic target localization algorithm that is executed by the cluster head. The localization results are used by the cluster head to query only a few sensors (out of those that report the presence of a target) for more detailed information. Simulation results are presented to demonstrate the effectiveness of the proposed approach.
518 citations
"Relative Pose Estimation using Rang..." refers methods in this paper
...And relative localization has been used for swarm control [10], target tracking [11] and target localization [12] applications....
[...]