Nonlinear estimation problems, such as range-only and bearing-only target tracking, are often addressed using linearized estimators, e.g., the extended Kalman filter (EKF). These estimators generally suffer from linearization errors as well as the inability to track multimodal probability density functions. In this paper, we propose a bank of batch maximum a posteriori (MAP) estimators as a general estimation framework that provides relinearization of the entire state trajectory, multihypothesis tracking, and an efficient hypothesis generation scheme. Each estimator in the bank is initialized using a locally optimal state estimate for the current time step. Every time a new measurement becomes available, we relax the original batch-MAP problem and solve it incrementally. More specifically, we convert the relaxed one-step-ahead cost function into polynomial or rational form and compute all the local minima analytically. These local minima generate highly probable hypotheses for the target's trajectory and hence greatly improve the quality of the overall MAP estimate. Additionally, pruning of least probable hypotheses and marginalization of old states are employed to control the computational cost. Monte Carlo simulation and real-world experimental results show that the proposed approach significantly outperforms the standard EKF, the batch-MAP estimator, and the particle filter.

Nonlinear estimation problems, such as bearingonly tracking, are often addressed using linearized estimators, e.g., the extended Kalman filter (EKF). These estimators generally suffer from linearization errors as well as the inability to track multimodal probability density functions (pdfs). In this paper, we propose a bank of batch maximum a posteriori (MAP) estimators as a general estimation framework that provides relinearization of the entire state history, multi-hypothesis tracking, and an efficient hypothesis generation scheme. Each estimator in the bank is initialized using a locally optimal state estimate for the current time step. Every time a new measurement becomes available, we convert the nonlinear cost function corresponding to this relaxed one-step subproblem into polynomial form, allowing to analytically and efficiently compute all stationary points. This local optimization generates highly probable hypotheses for the target trajectory and greatly improves the quality of the overall MAP estimate. Additionally, pruning and marginalization are employed to control the computational cost. Monte Carlo simulations and real-world experiments show that the proposed approach significantly outperforms the EKF, the standard batch MAP estimator, and the particle filter (PF), in terms of accuracy and consistency.

In this paper, we address the problem of determining the relative position and orientation (pose) of two robots navigating in 2D, based on known egomotion and noisy robot-to-robot distance measurements. We formulate this as a weighted Least Squares (WLS) estimation problem, and determine the exact global optimum by directly solving the multivariate polynomial system resulting from the first-order optimality conditions. Given the poor scalability of the original WLS problem, we propose an alternative formulation of the WLS problem in terms of squared distance measurements (squared distances WLS or SD-WLS). Using a hybrid algebraic-numeric technique, we are able to solve the corresponding first-order optimality conditions of the SD-WLS in 125 ms in Matlab. Both methods solve the minimal (3 distance measurements) as well as the overdetermined problem (more than 3 measurements) in a unified fashion. Simulation and experimental results show that the SD-WLS achieves performance virtually indistinguishable from the maximum likelihood estimator, and significantly outperforms current algebraic methods.

In this paper, we provide a study of motion-induced 3-D extrinsic calibration based on robot-to-robot sensor measurements. In particular, we introduce algebraic methods to compute the relative translation and rotation between two robots using known robot motion and robot-to-robot 1) distance and bearing, 2) bearing-only, and 3) distance-only measurements. We further conduct a nonlinear observability analysis and provide sufficient conditions for the 3-D relative position and orientation (pose) to become locally weakly observable. Finally, we present a nonlinear weighted least-squares estimator to refine the algebraic pose estimate in the presence of noise. We use simulations to evaluate the performance of our methods in terms of accuracy and robustness.

This paper addresses the problem of cooperative localization (CL) under severe communication constraints. Specifically, we present minimum mean square error (MMSE) and maximum a posteriori (MAP) estimators that can process measurements quantized with as little as one bit per measurement. During CL, each robot quantizes and broadcasts its measurements and receives the quantized observations of its teammates. The quantization process is based on the appropriate selection of thresholds, computed using the current state estimates, that minimize the estimation error metric considered. Extensive simulations demonstrate that the proposed Iteratively-Quantized Extended Kalman filter (IQEKF) and the Iteratively Quantized MAP (IQMAP) estimator achieve performance indistinguishable of that of their real-valued counterparts (EKF and MAP, respectively) when using as few as 4 bits for quantizing each robot measurement.

In this paper, we present the vision-aided inertial navigation (VISINAV) algorithm that enables precision planetary landing. The vision front-end of the VISINAV system extracts 2-D-to-3-D correspondences between descent images and a surface map (mapped landmarks), as well as 2-D-to-2-D feature tracks through a sequence of descent images (opportunistic features). An extended Kalman filter (EKF) tightly integrates both types of visual feature observations with measurements from an inertial measurement unit. The filter computes accurate estimates of the lander's terrain-relative position, attitude, and velocity, in a resource-adaptive and hence real-time capable fashion. In addition to the technical analysis of the algorithm, the paper presents validation results from a sounding-rocket test flight, showing estimation errors of only 0.16 m/s for velocity and 6.4 m for position at touchdown. These results vastly improve current state of the art for terminal descent navigation without visual updates, and meet the requirements of future planetary exploration missions.

In this paper, we develop an algorithm for determining the relative position and attitude of two robots moving in 3D, using only dead-reckoning and inter-robot distance measurements. From the mechanical analogue of parallel manipulators, it is known that this problem has 40 solutions when six distance measurements are available. These general solutions are not known in closed-form, and existing closed-form solutions require additional bearing sensors, or impose strong constraints on the geometric structure of the manipulator (in our case, the robots’ trajectories). This paper presents, for the first time, an efficient, algebraic algorithm to solve the relative pose using 10 distance measurements, without imposing any constraints on the robots’ motion. We further present a weighted leastsquares refinement step, and validate our algorithm in various simulations, demonstrating its efficiency and accuracy.

Bearing-only SLAM describes the process of simultaneously localizing a mobile robot while building a map of the unknown surroundings, using bearing measurements to landmarks as the only available exteroceptive sensor information. Commonly, the position of map features is estimated along with the robot pose. However, consistent initialization of these positions is a difficult problem in bearing-only SLAM, in particular for distant landmarks. In previous approaches, measurements to remote landmarks often had to be discarded, thus losing valuable orientation information. In this paper, we present for the first time a unifying framework allowing for non-delayed initialization of both nearby and distant features. This is made possible by a four-element landmark parametrization, combined with a constraint-based inferred measurement

This work presents an approach to optimizing entire trajectories for a group of mobile robots that use one another as localization beacons. The cost function we seek to optimize is a measure of localization uncertainty (as opposed to distance travelled or time). Our initial findings show that, for example, it is possible to improve on the intuitive equilateral-triangle formation for three robots.