Ke X. Zhou

Optimal Motion Strategies for Range-Only Constrained Multisensor Target Tracking

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.

Interrobot Transformations in 3-D

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.

A hybrid estimation framework for Cooperative Localization under communication constraints

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.

Optimal motion strategies for range-only distributed target tracking

In this paper we study the problem of optimal trajectory generation for a team of mobile robots that tracks a moving target using range-only measurements. We propose an adaptive-relaxation algorithm for determining the set of feasible locations that each robot must move to in order to collect the most informative measurements; i.e., distance measurements that minimize the uncertainty about the position of the target. We prove that the motion strategy that minimizes the trace of the position error covariance matrix is equivalent to the one that minimizes its maximum eigenvalue. The proposed method is applicable regardless of the process model employed for describing the motion of the target while its computational complexity is linear in the number of robots. Extensive simulation results are presented, demonstrating that the performance attained with the proposed method is comparable to that obtained with exhaustive search whose computational cost is exponential in the number of robots

Optimized motion strategies for localization in leader-follower formations

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.

A bank of maximum a posteriori estimators for single-sensor range-only target tracking

In this paper, we study estimation consistency of single-sensor target tracking using range-only measurements. We show analytically that the cost function minimized by the iterated extended Kalman filter (IEKF) has up to three local minima, which can potentially result in inconsistency or even divergence. To address this issue, we instead propose a bank of maximum a posteriori (MAP) estimators to determine the target state-space trajectory. In particular, we use the local minima of the IEKF cost function at each time step as highly accurate initial hypotheses to start a bank of iterative nonlinear optimizations. Moreover, we employ pruning and marginalization to control computational complexity. Extensive Monte Carlo simulations show that the proposed algorithm significantly outperforms the IEKF, the unscented Kalman filter (UKF), the bank of IEKFs, the particle filter (PF), and the standard MAP, both in terms of accuracy and convergence speed.

Bearing-only target tracking using a bank of MAP estimators

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.

A Bank of Maximum A Posteriori (MAP) Estimators for Target Tracking

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 targets 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.

A sparsity-aware QR decomposition algorithm for efficient cooperative localization

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.