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.

This paper focuses on reducing the computational complexity of the extended Kalman filter (EKF)-based multi-robot cooperative localization (CL) by taking advantage of the sparse structure of the measurement Jacobian matrix H. In contrast to the standard EKF update, whose complexity is up to O(N4) (N is the number of robots in a team), we introduce a Modified Householder QR algorithm which fully exploits the sparse structure of the matrix H, and prove that the overall complexity of the EKF update, based on our QR factorization scheme, reduces to O(N3). Finally, we validate the Modified Householder QR algorithm through extensive simulations, and demonstrate its superior performance both in terms of accuracy and CPU runtime, as compared to the current state-of-the-art QR decomposition algorithm for sparse matrices.

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 consider the problem of multi-centralized Cooperative Localization (CL) under severe communication constraints, i.e., when each robot can communicate only a single bit per real-valued (analog) measurement. Existing approaches, such as those based on the Sign-of-Innovation Kalman filter (SOI-KF) and its variants, require each robot to process quantized versions of both its local (i.e., recorded by its own sensors) and remote (i.e., collected by other robots) measurements. This results in suboptimal performance since each robot has to discard information that is available in its own analog measurements. To address this limitation, we introduce a novel hybrid estimation scheme that enables each robot to process both quantized (from remote sensors) and analog (from its own sensors) measurements. Specifically, we first present the hybrid (H)-SOI-KF, a direct extension of the SOI-KF, for processing both types of measurements. Secondly, we introduce the modified (M)H-SOI-KF, that uses an asymmetric encoding/decoding scheme to incorporate additional information during quantization (based on the hybrid estimates locally available to each robot), resulting in substantial accuracy improvement. Lastly, we present extensive simulations which demonstrate that both hybrid estimators not only outperform the SOI-KF, but also achieve accuracy comparable to that of the standard (analog) centralized Kalman filter.

This paper addresses the problem of determining the optimal robot trajectory for localizing a robot follower in a leader-follower formation using robot-to-robot distance or bearing measurements. In particular, maintaining a perfect formation has been shown to reduce the localization accuracy (as compared to moving randomly), or even leads to loss of observability when only distance or bearing measurements are available and the robots move on parallel straight lines. To address this limitation, we allow the follower to slightly deviate from its desired formation-imposed position and seek to find the next best location where it should move to in order to minimize the uncertainty about its relative, with respect to the leader, position and orientation estimates. We formulate and solve this non-convex optimization problem analytically and show, through extensive simulations, that the proposed optimized motion strategy leads to significant localization accuracy improvement as compared to competing approaches.

In this paper, we study the problem of optimal trajectory generation for a team of heterogeneous robots moving in a plane and tracking a moving target by processing relative observations, i.e., distance and/or bearing. Contrary to previous approaches, we explicitly consider limits on the robots' speed and impose constraints on the minimum distance at which the robots are allowed to approach the target. We first address the case of a single tracking sensor and seek the next sensing location in order to minimize the uncertainty about the target's position. We show that although the corresponding optimization problem involves a nonconvex objective function and a nonconvex constraint, its global optimal solution can be determined analytically. We then extend the approach to the case of multiple sensors and propose an iterative algorithm, i.e., the Gauss-Seidel relaxation (GSR), to determine the next best sensing location for each sensor. Extensive simulation results demonstrate that the GSR algorithm, whose computational complexity is linear in the number of sensors, achieves higher tracking accuracy than gradient descent methods and has performance that is indistinguishable from that of a grid-based exhaustive search, whose cost is exponential in the number of sensors. Finally, through experiments, we demonstrate that the proposed GSR algorithm is robust and applicable to real systems.

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.

In this paper, we study the problem of optimal trajectory generation for a team of mobile robots tracking a moving target using distance and bearing measurements. Contrary to previous approaches, we explicitly consider limits on the robots' speed and impose constraints on the minimum distance at which the robots are allowed to approach the target. We first address the case of a single sensor and show that although this problem is non-convex with non-convex constraints, in general, its optimal solution can be determined analytically. Moreover, we extend this approach to the case of multiple sensors and propose an iterative algorithm, Gauss-Seidel-relaxation (GSR), for determining the set of feasible locations that each sensor should move to in order to minimize the uncertainty about the position of the target. Extensive simulation results are presented demonstrating that the performance of the GSR algorithm, whose computational complexity is linear in the number of sensors, is indistinguishable of that of a grid-based exhaustive search, with cost exponential in the number of sensors, and significantly better than that of a random, towards the target, motion strategy.

In this paper, we study the problem of optimal trajectory generation for a team of mobile sensors tracking a moving target using distance-only measurements. This problem is shown to be NP-hard, in general, when constraints are imposed on the speed of the sensors. We propose two algorithms, modified Gauss-Seidel relaxation and linear programming (LP) relaxation, for determining the set of feasible locations that each sensor should move to in order to collect the most informative measurements; i.e., distance measurements that minimize the uncertainty about the position of the target. These algorithms are applicable regardless of the process model that is employed for describing the motion of the target, while their computational complexity is linear in the number of sensors. Extensive simulation results are presented demonstrating that the performance attained with the proposed methods is comparable to that obtained with grid-based exhaustive search, whose computational cost is exponential in the number of sensors, and significantly better than that of a random, toward the target, motion strategy.

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.