Bearing-only target tracking using a bank of MAP estimators

Nonlinear estimation problems, such as bearing-only 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.