Bearingonly Tracking Using Bank of MAP Estimators Guoquan P
126K - views

Bearingonly Tracking Using Bank of MAP Estimators Guoquan P

Huang and Stergios I Roumeliotis Multiple Autonomous Robotic Systems Labroratory Technical Report Number 20100001 February 2010 Dept of Computer Science Engineering University of Minnesota 4192 EECS Building 200 Union St SE Minneapolis MN 55455 Tel

Tags : Huang and Stergios
Download Pdf

Bearingonly Tracking Using Bank of MAP Estimators Guoquan P




Download Pdf - The PPT/PDF document "Bearingonly Tracking Using Bank of MAP E..." is the property of its rightful owner. Permission is granted to download and print the materials on this web site for personal, non-commercial use only, and to display it on your personal computer provided you do not modify the materials and that you retain all copyright notices contained in the materials. By downloading content from our website, you accept the terms of this agreement.



Presentation on theme: "Bearingonly Tracking Using Bank of MAP Estimators Guoquan P"— Presentation transcript:


Page 1
Bearing-only Tracking Using Bank of MAP Estimators Guoquan P. Huang, and Stergios I. Roumeliotis Multiple Autonomous Robotic Systems Labroratory Technical Report Number -2010-0001 February 2010 Dept. of Computer Science & Engineering University of Minnesota 4-192 EE/CS Building 200 Union St. S.E. Minneapolis, MN 55455 Tel: (612) 625-2217 Fax: (612) 625-0572 URL: http://www.cs.umn.edu/~ghuang
Page 3
Bearing-only Tracking Using Bank of MAP Estimators Guoquan P. Huang, and Stergios I. Roumeliotis Multiple Autonomous Robotic Systems Laboratory, TR-2010-0001 February 2010

Abstract Nonlinear estimation problems, such as bearing-only tracking, are often addressed using linearized estimators (e.g., the extended Kalman lter (EKF)). The resulting linearization errors of these estimators as well as their inability to track multimodal probability density functions (pdfs) greatly diminish their performance. To address these issues, we employ relinearization provided by the batch maximum a posteriori (MAP) estimator to reduce linearization errors, and introduce a general estimation framework (a bank of MAP estimators) to track multiple modes of the posterior

pdf. The key idea of this approach is to convert the nonlinear cost function into polynomial form and then to analytically compute all its stationary points for the current time step. Using the local minima as the most probable hypotheses of the state trajectory eectively focuses available computational resources. In addition, we employ pruning and marginalization to control the computational cost. Monte Carlo simulations and real-world experiments show that the proposed approach signicantly outperforms the EKF, the standard batch MAP estimator, and the particle lter

(PF), in terms of accuracy and consistency. 1 Introduction Bearing-only target tracking is the problem of estimating the kinematic state (e.g., position, velocity, etc.) of a moving target using external (mobile) sensors that measure bearing to the target. This problem has attracted signicant interest over past decades, since it arises in a variety of applications, such as submarine tracking using towed-array sonar and aircraft surveillance using radar in a passive mode [2,3]. Examples of recent research include design of new estimators and adaptive control of sensor motions [4,5]. In

this paper, we focus on the case of tracking a single target from a single mobile sensor, to illustrate our methodology of designing consistency-improved estimators. Bearing-only tracking, a classical nonlinear estimation problem, is often addressed by linearized lters (e.g., the extended Kalman lter (EKF)). However, their inability to track a multimodal probability density function (pdf), as well as the linearization errors they incur, can greatly diminish the tracking performance. One popular way to reduce linearization errors is to use the iterated EKF (IEKF) that

iteratively relin- earizes the measurement function until its estimate converges [3]. Alternatively, the unscented Kalman lter (UKF) employs the unscented transform to reduce the estimation errors due to linearization [6]. However, all these EKF-like estimators are only able to (explicitly or implicitly) linearize the underlying nonlinear system around the state estimate for the current time step, and thus are sensitive to linearization errors. In eect, this is the case for any nonlinear ltering approach, since lters inherently marginalize all but the current

state and thus xes past linearization points. It cannot update the corresponding old states when new measurements become available for improving their estimates. In contrast, a batch maximum a posteriori (MAP) estimator [7] computes the estimates for the states at all time steps using all the available measurements. It allows continuous relinearization of the entire state trajectory, greatly reducing lineariza- tion errors. It should be pointed out that like the EKF and its variants, a batch MAP estimator can only track a single mode of the posterior pdf, which, however, is often

multimodal due to the nonlinear nature of the problem. This can degrade its performance. Albeit that, only few estimators, such as the multi- hypothesis EKF (MHEKF) [4], and the particle lter (PF) [8], are specically designed to treat multimodal Note that bearing-only tracking is related but dierent from robot localization [1] in the sense that sensors self-localization is not considered here, i.e., the sensor pose (position and orientation) is assumed known.
Page 4
distributions by simultaneously tracking a set of dierent state estimates. However,

most of the time these hypotheses are generated randomly, wasting in eect a considerable portion of the computational resources. In this paper, based on the conjecture that one fundamental cause for the estimators inconsistency is the inability of the estimator to track a multimodal posterior pdf, we introduce a general estimation framework to mitigate this. The key idea is to convert the nonlinear cost functions into polynomial forms, and subsequently employ algebraic-geometry techniques [9] to compute all the stationary points and thus all modes of the pdf. Within this framework, a

bank of MAP estimators is developed for bearing-only tracking, which simultaneously permits linearization-error reduction and ecient, high-quality hypothesis generation to track multiple modes of the posterior pdf. Specically, by converting the batch MAP problem into a system of multivariate polynomials, we nd that it is, in general, computationally intractable to solve the resulting polynomial system analytically. We therefore employ an relaxation by keeping previous state estimates unchanged and then incrementally solving a one-step minimization problem for the

current state at very time step when a new measurement becomes available. This minimization problem is solved analytically by transforming the bearing measurement and thus the cost function into rational (polynomial) forms. The analytically computed local minima enable us to eciently focus available resources on the most probable hypotheses of the state trajectory. Furthermore, since the number of possible combinations of the local minima can grow exponentially over time, we employ eective pruning and marginalization schemes to limit the hypothesis-growth rate, thus ensuring

low, resource-adaptive computational cost. Apart from bearing-only tracking treated in this paper, the proposed approach is applicable to a broad class of nonlinear estimation problems in robotics and computer vision that can be expressed in (or converted into) polynomial form. For instance, in our recent work [10], it was successfully applied to signicantly improve the performance of range-only tracking. In this paper, we generalize this idea to a more challenging problem and provide a detailed analysis as well as experimental validation. 2 Related Work Bearing-only tracking has been

studied for decades and many algorithms of both batch and recursive types have been proposed in the literature [3,11]. Among these algorithms, the EKF is one of the most widely used methods. Due to the fact that the EKF is unable to relinearize the nonlinear system when new measurements become available and thus can result in large linearization errors, the EKF gives unsatisfactory performance. This has given rise to renements of the EKF, e.g., the modied-polar coordinates EKF [12] and the shifted Rayleigh lter (SRF) [5]. However, both of these EKF variants can only

track a single mode of the posterior pdf of the target state and thus suer from the same problem as the EKF, i.e., they can potentially track an inaccurate mode of the posterior pdf and hence become inconsistent. To mitigate the aforementioned issue, an MHEKF was proposed in [4] to track multiple hypotheses of the target state. The MHEKF makes an assumption about the minimum and maximum distance between the sensor and target and divides this range interval into a number of subintervals, each representing a hypothesis regarding the true range of the target. A bank of independently

operating range-parameterized EKFs is thus created, each designed for one of the hypotheses and receiving the same bearing measurement. The MHEKF described in [4] determines a xed number of EKFs at the rst available measurement. This idea was extended in [13] so that the lter bank can dynamically change its size at each time step based on the current measurement likelihood. Since no lter in the MHEKF can guarantee computing the globally optimal estimate (due to the multimodal nature of the posterior pdf and its inability to relinearize the nonlinear measurement

function around past states), this approach can also become inconsistent and diverge. Note also that this method assumes prior knowledge about the maximum target distance, which might not always be available in real applications. More importantly, this approach does not provide a measurable criterion about how many partitions are needed in the assumed range interval and where to choose them. In contrast, our proposed bank of MAP estimators selects most probable hypotheses of the target trajectory based on local optimality at each time step. Considerable attention has recently been paid to the

PF [8,14{16], due to its capability of solving nonlinear estimation problems with multimodal pdfs. In the standard PF, each particle represents a hypothesis of the target state, weighted by its measurement likelihood. If the particles sample the state space suciently, the PF will converge to the true distribution. However, the particles are usually initialized randomly, and if far TR-2010-0001
Page 5
from a mode of the pdf, their weights will decay quickly and lead to particle depletion, even if resampling is employed. Therefore, in order to converge to meaningful estimates,

the PF requires a large number of particles, thus resulting in substantial computational demands. In contrast, the proposed bank of MAP estimators analytically computes all modes of the posterior pdf at the current time step and eciently focuses the computational resources on the most probable trajectory hypotheses. This will become more clear in the next sections. 3 Problem Formulation Consider a single sensor that moves in a plane and estimates the state (position, velocity, etc.) of a moving target by processing noisy bearing measurements. In this work, we study the case of global

tracking, i.e., the position of the target is expressed with respect to a xed (global) frame of reference, instead of a relative sensor-centered one. We hereafter assume known sensor position and orientation, as well as known parameters of a stochastic target motion model. The state vector of the target at time-step , is dened as a vector of dimension 2 , where 1 is the highest-order time derivative of the target position described by the known stochastic motion model, and can include components such as position, velocity, acceleration, etc.: (1) ;k (2) where is the target

position expressed in the global frame of reference, while ;k denotes all the higher-order time derivatives of the target position. In the following, we present the target stochastic motion model and the sensor bearing measurement model that will be used throughout the paper. 3.1 Motion Model We consider the case where the target moves randomly but assume that the stochastic model describing the motion of the target (e.g., constant acceleration or constant velocity [3]) is known. In particular, the discrete-time state propagation equation is generically given by the following linear form: (3)

where is zero-mean white Gaussian noise with covariance . The state transition matrix, and the process noise Jacobian, , that appear in the preceding expressions depend on the motion model used [3]. In this work, these can be arbitrary, but known, matrices, since no assumptions on their properties are made. 3.2 Measurement Model In this work, we focus on the nonlinear case where a single sensor measures its bearing angle to the target. Specically, the measurement at time-step , is given by: = atan2 (( )) (4) ) + (5) where is the known sensor pose expressed in the global frame of

reference, and is zero-mean white Gaussian measurement noise, with variance As will become evident later, our proposed approach does not depend on the particular selection of the target stochastic motion model. TR-2010-0001
Page 6
3.3 Batch MAP Estimator The MAP estimator utilizes all available information to estimate the entire history of states (i.e., trajectory) of the target, given by stacking all target states in the time interval [0 ;k ] (cf. (1)), i.e., 0: (6) The information used in the MAP includes: (i) the prior information about the initial target state described by a

Gaussian pdf with mean ^x and covariance , (ii) the target motion-model information (cf. (3)), and (iii) the sensor distance measurements (cf. (5)). The MAP estimator computes the entire state estimates that maximize the posterior pdf: 0: 1: =1 (7) where 1: denote all the measurements recorded by the sensor in the time interval [0 ;k ]. Using Gaussian assumptions for the target motion noise and the measurement noise (cf. (3) and (5), respectively), this pdf (7) can be written as: 0: 1: (8) (2 exp jj ^x jj =1 (2 exp jj jj =1 (2 exp jj jj The maximization of (8) is equivalent to the maximization

of its logarithm, which, in turn, is equivalent to the minimization of the following cost function: 0: ) = jj ^x jj =1 jj jj =1 jj jj (9) where we have employed the notation jj jj and . Since 0: ) is a nonlinear function, a standard approach for its optimization is to employ Gauss-Newton iterative minimization [17]. Specically, at the -th iteration of this method, a correction, 0: , to the current estimate, ^x 0: , is computed by minimizing the second-order Taylor-series approximation of the cost function, i.e., ^x 0: 0: ^x 0: ) + 0: 0: 0: (10) where ^x 0: ) and ^x 0: ) are the

gradient and Hessian of ) with respect to 0: evaluated at ^x 0: , respectively. The value 0: that minimizes (10) is found by solving the following linear system: 0: (11) We now examine the structures of the gradient and the Hessian. Specically, the gradient can be obtained as: (2 ^x =1 =1 (12) Throughout this paper the subscript refers to the estimate of a quantity at time-step , after all measurements up to time-step have been processed. ^ is used to denote the estimate of a random variable , while ~ is the error in this estimate. Finally, and denote matrices of zeros and ones,

respectively, while is the identity matrix. TR-2010-0001
Page 7
where (2 (2 is employed to adjust the dimension of the 2 -dimensional prior to the dimension of the state 0: . In the above expression, and are the respective Jacobians of the measurement and motion models (cf. (5) and (3), respectively), with respect to 0: , evaluated at ^x 0: . It is important to note that both the measurement function and the target motion model involve only a very few states (i.e., the measurement only depends on the target position when it is observed, while the target motion only depends on two

consecutive states). Thus, the structure of both and are sparse: (13) (2 (14) On the other hand, the Hessian matrix, , is approximated in the Gauss-Newton method by: (2 (2 =1 =1 (15) which is good approximation for small-residual problem [17]. Due to the sparse structure of the matrices and , the matrix is sparse, which can be exploited to speed-up the solution of the linear system in (11) [17]. Even though the MAP estimate is optimal up to linearization errors [7], convergence to the global minimum cannot be guaranteed unless the initial estimate ^x (0) 0: is within the region of attraction

of the optimum point. This is a limitation of most iterative algorithms used for solving nonlinear optimization problem such as (9), since, in general, there exists no systematic method for determining an initial estimate ^x (0) 0: that can always ensure convergence to the global optimum. Thus, the MAP estimate for the bearing- only target tracking can diverge if no accurate initial estimate is provided. This is due to the fact that the cost function (9) has multiple local minima and the initial estimate for the current time step provided to the MAP estimator (e.g., via the propagation

equations of the EKF) is generally not within the region of attraction of the global minimum. 4 Incrementally Solving the Batch MAP Optimization Problem In this section, we present an incremental solution to the batch MAP problem of minimizing (9). Since it is in general intractable to analytically solve the batch MAP problem, we relax it and incrementally solve a one-step minimization problem for the current state estimate analytically , at every time step when a new measurement becomes available. This analytic optimization is carried out by converting the nonlinear cost function into

rational (polynomial) form which is then solved using algebraic geometry techniques. 4.1 Relaxation of the Batch MAP Problem By transforming the trigonometric function of the bearing measurement (see (4)) into rational form, we can convert the KKT optimality conditions of the batch MAP problem into a polynomial system that has +1) variables (see Section 4.2). It is clear that the number of variables increases linearly with respect to the time horizon . However, since the complexity of solving multivariate polynomial systems is exponential in the number of variables [18], it is in general

computationally intractable to solve the batch MAP problem analytically (e.g., by using tools from algebraic geometry [9] to compute all stationary points). For this reason, we relax the batch MAP problem and solve it incrementally. We start by noting that the cost function (9) can be written as: 0: ) = 0: ) + jj jj jj jj (16) Since the motion model (see (3)) is linear, at every time step when a new measurement becomes available, we can combine all previous information about the targets trajectory to provide a prior for the new current TR-2010-0001
Page 8
state, , based on the

following propagation equations: ^x ^x (17) (18) Thus, the new state estimates can be computed incrementally by solving the following one-step minimization problem: min ^x ^x ) + )) (19) 4.2 Analytic Determination of the Local Minima Observing that the bearing measurement depends only on the target position (see (4)), we can decouple the target position, , and the remaining states, ;k , in solving (19), so as to simplify the ensuing derivations. Specically, using the following partitioning of the information matrix of the full state, pp pd dp dd , the cost function of (19) can be

written as: ) = ^p pp ^p ) + ;k ^x ;k dd ;k ^x ;k ) + ^p pd ;k ^x ;k ) + )) (20) We note that min ;k ;k ) = min min ;k ;k Thus, we rst solve for ;k by setting the gradient of (20) with respect to ;k to zero, and obtain: ;k ^x ;k dd dp ^p (21) Substitution of (21) into (20) yields: ) = ^p pp ^p ) + )) (22) where pp is the covariance matrix corresponding to the target position, obtained by partitioning the covariance matrix as pp pd dp dd . In (22), we have employed the identity, pp pp pd dd dp , which can easily be veried using the block matrix inversion lemma [19]. We thus see

that the problem (19) can be decoupled as follows: We rst solve for ^p by minimizing (22), and from that we then solve linearly for ^x ;k using (21). It is also important to note that the size of the nonlinear problem has dramatically decreased, from 2 , for (19), to a constant size of 2 for minimizing (22). Moreover, the analytic solution for the target position is independent of its higher-order time derivatives, regardless of the stochastic target motion model. Now we focus on the analytic solution of (22). In order to use an algebraic geometry approach, we transform the bearing

measurement function (4) into a rational form. Specically, after moving the sensor TR-2010-0001
Page 9
−1 −0.5 0.5 1.5 0.002 0.004 0.006 0.008 0.01 0.012 0.014 0.016 0.018 ˇz p(ˇz) Exact pdf Guass. approx. Figure 1: An example of approximating the pdf of transformed measurements by a Gaussian pdf. In this case, = 0 5 and = 10 deg. In addition, the Kullback-Leibler distance (KLD) between these two pdfs is only 0.0447, which indicates the dierence between the two distributions is small. orientation term to the right hand side of (4), we apply the

tangent function on both sides and obtain the following transformed measurement: tan( (23) = tan (atan2 (( )) + By denoting atan2 (( )), considering ; ], and following the standard formulas to compute the pdf of functions of random variables [20], the pdf of the transformed measurement is given by: ( ) = (tan ( ); ; )+ (tan ( ; 1+ if (tan ( ); ; )+ (tan ( )+ ; 1+ if (24) where ; ) denotes the Gaussian pdf of a random variable with mean and variance . Clearly, ( ) is not Gaussian (it results from the tangent of a Gaussian random variable), but it can be well approximated by a

Gaussian pdf by matching rst and second order moments. This is done by linearizing (23) around the expected value of the noise, i.e., + (25) where sec , is zero-mean white Gaussian noise with variance sec , i.e., N (0 ; ). We term this approximation (25) an inferred measurement which is in the desired rational form. As illustrated in Fig. 1, this approximation is reasonably accurate, particularly for high signal-to- noise ratios. Moreover, the local minimum of (22) attained based on the inferred measurement is very close to that based on the corresponding original bearing

measurement. This can be seen from Fig. 2, where one of the local minima using the inferred measurement almost coincides with the IEKF posterior estimate (which corresponds to the local minimum determined by applying the Gauss-Newton method [21,22]) using the original bearing measurement. This further shows that the inferred measurement is a reasonably good approximation to the original bearing measurement. Moreover, the inferred measurement is used only for nding hypotheses of the trajectory, not for estimating the state per se, whose estimates will be updated by the batch MAP

estimators using all available original bearing measurements. Note also that this inferred measurement model (25) does not consider the special case of . However, this case has low probability of occurrence in practice, and can be easily avoided through an appropriate coordinate transformation. TR-2010-0001
Page 10
−25 −20 −15 −10 −5 10 15 20 25 −10 10 20 30 40 50 x (m) y (m) True target True sensor Prior estimate err. ellipse (prior) Posterior estimate err. ellipse (post.) meas. sect. Local minima Figure 2: Illustrative problem for a single time

step bearing-only tracking. The magenta crosses indicate the locations of two local minima as computed by (29)-(30). The MAP estimate initialized with the prior estimate converges to the local minimum with larger error with respect to ground truth. Note that the MAP estimate is computed based on the original (not inferred) measurements. The approximation (25) used in the inferred measurements introduces a slight oset in the analytic local minima. In what follows we use the inferred measurement (25) (instead of (5)) to compute the analytic solutions for minimizing (22). In particular,

with pp , (22) can be written as: ;y ) = )( (26) Setting the derivatives of ;y ) with respect to the two optimization variables to zero, and performing simple algebraic manipulations, we have: @c @x ) + ) + = 0 )( ) + )( = 0 (27) @c @y ) + = 0 ) + [ )] = 0 (28) From (28), we can compute in terms of as follows: )+ 1 + 1 + (29) TR-2010-0001
Page 11
Substitution of (29) into (27) yields a rational equation, whose denominator is always non-zero. Thus, we only need to focus on the numerator which is an eighth-order univariate polynomial in 0 = ) = =0 (30) where = 0 ;:::; 8, are the

coecients expressed in terms of ;s ;s ;s ;x , and + 2 zs zs + 2 + 2 zs zx zs (31) = 5 zs + 3 10 + 3 zs + 3 + 7 zs + 4 zx + 4 + 4 zs + 8 (32) = 10 18 + 9 zx + 6 + 3 + 10 zs + 3 10 zs 12 + 12 + 7 + 3 + 10 zs 10 zs + 3 zx + 12 + 3 zx zx + 21 + 20 21 + 2 (33) zx 20 + 22 + 21 10 zs + 10 zs + 8 + 10 zs 10 zs 35 20 + 35 + 14 21 + 4 + 4 (34) = 35 35 + 5 zs + 35 zs + 2 + 2 13 + 20 10 + 2 + 5 zs + 3 16 35 zs (35) = 21 + 35 10 + 9 zs 21 + 3 35 zs zs zs (36) = 2 + 7 21 + 21 (37) + 7 (38) (39) The roots of ) are the eigenvalues of the corresponding 8 8 companion matrix [23]. Although there exist 8

solutions for and thus 8 solutions for , as it depends injectively on (see (29)), we only need to consider the pairs ( ;y ) that correspond to real eigenvalues of the companion matrix. Moreover, since some of these solutions could be local maxima or saddle points, the second-order derivative test [24] is employed to extract the minima. Finally, once we determine all the local minima for the target position, we compute the corresponding estimates for the higher-order position derivatives via (21). TR-2010-0001 10
Page 12
The following lemma provides a smaller upper bound for the

maximum number of local minima, which is important since it signicantly impacts the computational complexity of our proposed method (see Section 5). Lemma 4.1. There are at most 7 local minima for (26) Proof. Using the Finite Dimensional Mountain Pass Theorem (see Theorem 5.2 in [25]), which states that there exists a third critical point which is not a local minimum between any two strict local minima for a coercive function, it is straightforward to show that our cost function (26) 2C nf is coercive, and thus at least one of the 8 critical points cannot be a local minimum, leaving a

maximum number of 7 local minima. Note that due to its rational form, the inferred measurement (25) is symmetric with respect to the sensor, which is not the case for the original bearing measurement (5). This can result in more local minima of (26) than those of (22). To discard the spurious local minima due to the symmetry of the inferred measurement, we employ the Mahalanobis distance test [3]. As a result, we have never observed more than four local minima in practice. 5 Bank of MAP Estimators for Bearing-only Tracking As discussed in the preceding section, due to the nonlinearity of the

(inferred) bearing measurement, the incremental one-step MAP problem (19) (and thus the original multi-step MAP problem (9)) has multiple local minima, corresponding to multiple modes of the posterior pdf. However, any iterative algorithm (e.g., Gauss-Newton) used in the batch MAP estimator converges only to a local minimum, unless the initial estimate ^x (0) 0: is within the region of attraction of the global optimum. This is a limitation of iterative algorithms used for solving nonlinear optimization problems such as minimization of (9), since, in general, there exist no systematic methods

for determining an initial estimate ^x (0) 0: that can always ensure convergence to the global optimum. Thus, the batch MAP estimate for bearing-only target tracking can become inconsistent and diverge if no accurate initial estimate is provided. Fig. 2 shows a typical example in which the posterior estimate of current state clearly converges to a local minimum with larger error compared to the ground truth. Since this inaccurate estimate will be used in subsequent propagation and update steps, the state estimates may eventually become inconsistent and diverge. To address this challenge, in

what follows we introduce a general estimation framework for nonlinear, multimodal problems (with polynomial cost functions). Specically, based on the analytic local minima to the incremental one-step MAP problems (see Section 4.2), we employ a bank of MAP estimators to nd and track the most probable modes of the posterior pdf (hypotheses of the target trajectory), so as to improve tracking performance. Specically, at time-step 1, we rst propagate the current state estimate corresponding to the -th solution and its covariance matrix, ^x and = 1 ;:::;m is the

number of estimators in the bank at time-step 1), through the propagation equations (17) and (18). Then, once a new bearing measurement becomes available, the propagated state estimate and covariance, ^x and , are used as the prior in (19). We subsequently employ the algebraic method presented in Section 4.2 to determine all the analytic local minima of (19), denoted by , 1 (see Lemma 4.1). Finally, for each of these solutions, we employ a MAP estimator that uses the latest estimates of the trajectory corresponding to this solution as the initial value, to rene the entire state

estimates ^x 0: up to current time-step (see (9)). This procedure recursively evolves over time, and at every time step, generates at most 7 (in practice up to 4 ) trajectory estimates. Thus, in the end, we will have multiple MAP estimates, among which the one with the least cost is selected as the best estimate for the global optimum (and thus for the true state). Fig. 3 shows the block diagram of the proposed bank of MAP estimation algorithm for the -th estimated trajectory, while Algorithm 1 outlines the main steps of the proposed algorithm. A function is coercive if it is bounded from

below and is proper in the sense that !1 for jj jj!1 TR-2010-0001 11
Page 13
Propagation Analytic Solver MAP MAP Figure 3: Block diagram of the -th MAP estimator in the bank(1 7). Algorithm 1 Bank of MAP for Bearing-only Tracking At each time-step Propagate the current target state estimate and covariance via (17) and (18). Analytically determine all the local minima of (19). For each of the local minima, rene the corresponding entire state (trajectory) estimates and covariance, by employing the MAP estimator that uses the latest state estimates corresponding to this solution

as the initial guess, and also compute the MAP cost (9). In the end, select the MAP estimate in the bank with the least cost as the resulting best estimate. 5.1 Computational Cost Reduction At this point, it is important to note that, in the worst case when multiple solutions exist for the one-step minimization problem at each time step (cf. (19)), the total number of the solutions grows exponentially with time, so does the number of initial estimates and thus the number of MAP estimators in the bank. Moreover, as the target continuously moves, the size of the entire state vector of each MAP

estimator increases linearly with time. In order to make the algorithm suitable for real-time applications, in what follows, we present an eective pruning scheme as well as the marginalization of old states, to reduce the computational complexity of the proposed bank of MAP estimators. 5.1.1 Pruning scheme We observe that in general if two MAP estimators in the bank have similar costs, their state estimates (i.e., the estimated trajectories) are quite close too. Thus, we rst aggregate the estimated trajectories whose costs are equal within a tolerance, and then choose the one

with the least cost as the representative of this aggregation and discard the others. In addition, we also employ the k-means algorithm [26] to cluster the remaining estimated trajectories into two groups (i.e., inlier and outlier) based on their costs and state estimates, and remove the outlier group which have larger costs. These two steps, aggregation and clustering, are repeated, until the number of MAP estimators in the bank is within the threshold, max 5.1.2 Marginalization of old states As the target continuously moves, the size of the state vector (or 0: ) in the MAP estimator con-

stantly increases (linearly in time). This is prohibitive for real-time operations. Therefore, we resort to Simulation results have shown that the aggregation most time is very eective so that there is no need to perform the clustering. TR-2010-0001 12
Page 14
marginalization of old states, which is derived from the perspective of minimization of the cost function [27]. In particular, suppose that during the time interval [0 ;k ], all available measurements are collected, and each MAP estimator of the MAP bank, using the initial estimate found by the approach presented in the

preceding section, is carried out at time-step . The old states 0: are marginalized out, while the states +1: remain active in the slide window. Then, as the target keeps moving and the sensor continuously collects new measurements in the time interval [ k;k ], the state vector of the MAP estimator is augmented by the new target states +1: . Thus, at time-step , the sliding window contains the states and , and the optimal MAP estimate (i.e., without marginalization), is computed by minimizing a cost function similar to (9): ) = ) = ) + (40) where we have decomposed the cost function into two

terms: ) that contains all quadratic terms that involve states in only, as well as terms involving one state in and one in ; and that contains all quadratic terms that involve states in only, states in only, or terms involving one state in and one in . It is important to note that, no quadratic terms jointly involving states in and exist, since the states marginalized at time-step do not participate in any measurement after that time. Thus, we have: min ) = min min = min ) + min (41) Now, we solve the minimization of (cf. (9)): ) = jj ^x jj =1 jj jj +1 =1 jj jj (42) Clearly, contains the

quadratic cost terms due to (i) the prior, (ii) the target motion model involving the marginalized target states and the rst target state in (i.e., +1 ), and (iii) the measurements that involve the marginalized target states. Due to the nonlinearity of the measurements, the second-order Taylor-series approximation to is employed: ^x ^x ) + ^x ^x ^x ^x ^x ^x (43) where and are the Jacobian and Hessian matrices of , evaluated at the MAP estimates of and at time-step (cf. (12) and (15)), which are the best available estimates since minimization takes place at time-step . We note that the

following block partitioning of the Jacobian and Hessian matrices of will be useful for ensuing derivations: ^x ^x ) = mm mr (44) ^x ^x ) = mm mr rm rr (45) where the dimensions of the blocks correspond to the dimensions of and , and the subscript denotes the fact that all quantities in these matrices are evaluated using the state estimates at time-step . We TR-2010-0001 13
Page 15
note that the cost function in (43) is a quadratic function of , and thus the optimal value of can be attained as: ^x mm mm mr ^x (46) And substitution in (43) yields the minimum value of min (47) ^x ) +

^x ^x where is a constant independent of and , and mr rm mm mm (48) rr rm mm mr (49) Thus, with (47) and (41), the minimization of the cost function, ), with respect to the entire history of states, is approximately equivalent to the minimization of the following cost function: ) = (50) ^x ) + ^x ^x +1 jj jj +2 jj jj It is important to note that the above cost function does not depend on . The approximation here lies in the fact that the term has been permanently approximated by the second-order Taylor series approximation of (43). This will introduce small errors in the MAP estimates for and

, but if the marginalized states are old, \mature" ones, with good estimates, the eect of the approximation will be small. On the other hand, the gain from employing this approximation is that the marginalized states as well as all measurements that directly involve them, can be discarded, thus yielding an algorithm with constant-time and constant-memory requirements. In particular, as proceeding in the full-state MAP, the minimization of ) at time-step (cf. (50)), can be carried out by the Gauss-Newton method (cf. Section 3.3). Specically, during the -th iteration, the

correction to the active states , is computed by solving the sparse linear system , where ^x +1 )) + +2 (51) +1 +2 (52) where and = 2 ) is the dimension of the states . By comparison of the structure of these equations with those of (12) and (15), which correspond to the full-MAP estimator, it becomes clear that the term expresses the prior information about the states , which arises due to TR-2010-0001 14
Page 16
−120 −100 −80 −60 −40 −20 20 40 60 −20 20 40 60 80 100 x (m) y (m) TARGET SRART TARGET END True sensor True target Figure 4: The

trajectories of the target and sensor obtained from one typical realization of the 100 Monte Carlo simulations. the marginalization of at time step . An important dierence of this set of equations compared to those of (12) and (15) is the fact that in (51), the prior information is expressed with two terms: in particular, the constant term appears in (51), while no such constant term appears in (12) (put dierently, the constant term in (12) is zero). This \extra term" in (51) is a consequence of the fact that the prior estimate ^x is not the optimal estimate that would arise

using the marginalized measurements. To see why this is the case, we note that the cost function cm in (42) does not have a minimum at ^x ^x , since its Jacobian, is generally not zero at ^x ^x . This occurs because the estimate ^x ^x has been computed using not only the marginalized measurements, which dene the cost function , but all the available measurements at time-step 6 Simulation Results A series of Monte Carlo simulations were conducted under various conditions, in order to demonstrate the capability of the proposed bank of MAP estimators to improve tracking performance. The

metrics used to evaluate estimators performance were: (i) the average root mean squared error (RMSE), and (ii) the normalized (state) estimation error squared (NEES) [3]. Both metrics are computed by averaging over all Monte Carlo runs for each time step. The average RMSE provides us with a concise metric of the accuracy of a given estimator, while the NEES is a metric for evaluating estimators consistency. By studying both the RMSE and NEES of all the estimators under consideration, we obtain a comprehensive picture of the estimators performance. In the following simulation tests, we

performed 100 Monte Carlo simulations, and compared several dierent estimators. During each Monte Carlo run, all the estimators process the same data, to ensure a fair comparison. The compared estimators were: (i) the standard EKF, (ii) the standard batch MAP estimator using the EKF estimates as the initial value as well as employing the marginalization process as in (iv), (iii) the sampling importance resampling (SIR)-PF with 2000 particles [8], and (iv) the proposed bank of MAP estimators with pruning ( max = 10) and marginalization (sliding window of 50 time steps). Note that

depending on dierent resampling schemes and dierent proposal distributions used by the PF, many variants exist, e.g., auxiliary PF, regularized PF, likelihood PF, etc. Interested readers are referred to [8, 16, 28] for an overview of the PF. In this simulation, we implemented the standard (conventional) SIR-PF that uses the state-transition prior distribution as the proposal distribution and employs systematic resampling at every time step. Moreover, to alleviate the particle depletion problem, we also dithered the process noise (increase noise covariance). We also examined

dierent resampling schemes such as Ripleys and stratied resampling [16], but found negligible performance increase. TR-2010-0001 15
Page 17
20 40 60 80 100 120 140 160 180 200 100 200 300 400 500 Position RMSE (m) EKF PF MAP MAP bank 20 40 60 80 100 120 140 160 180 200 10 20 30 40 50 60 Time Steps Velocity RMSE (m/sec) (a) 20 40 60 80 100 120 140 160 180 200 100 200 300 400 500 600 700 800 900 1000 Time Steps NEES EKF PF MAP MAP bank (b) Figure 5: Monte Carlo simulation results: (a) RMSE, (b) NEES. It is clear that the proposed algorithm performs substantially better

than its competitors, in terms of both accuracy (RMSE) and consistency (NEES). Note that for clarity of presentation, only the portions of the NEES lines that are within certain thresholds are plotted. For the results presented in this section, we adopted a zero-acceleration motion model for the target [3]: _x ) = Fx ) + Gw (53) where 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 1 ) = and ) = is zero-mean, white Gaussian noise with covariance ), where = 2 sec Hz , and ) is the Dirac delta function. In the implementation, we discretized this continuous-time system model (53) with time step = 0

1 sec. The initial true target state was 0 0 5 5 , while the initial estimate of the target state was randomly generated from a Gaussian pdf, ), where = 10 is the initial covariance of the state estimate. Similar to [11], we chose a circular sensor trajectory with perfectly known poses for this simulation. Fig. 4 shows the trajectories of the target and sensor in one typical realization of Monte Carlo simulations. In this simulation, the standard deviation of the bearing-measurement noise was set to 10 deg. Fig. 5 shows the Monte Carlo results of the four estimators. As evident from this

gure, the standard EKF estimates are inaccurate (RMSE), diverge from the ground truth, and become inconsistent (NEES). Thanks to the continuous relinearization of the standard batch MAP estimator that incrementally uses the EKF estimates as the initial guess, it signicantly improves the performance. As expected, the PF attains better estimation accuracy than the EKF. This is due to the fact that each particle in the PF essentially represents a hypothesis of the target state, and thus the PF is more likely to converge to the optimal solution. However, it does not work as well as

the standard batch MAP estimator, in part because it does not allow smoothing the old state estimates using new available measurements (though it inherently can solve the smoothing problem, but will demand substantial complexity [16]). Note also that the NEES of the PF is not necessarily better than that of the EKF, primarily due to the numerical issue incurred in the simulation that the covariance matrices of PF computed from particles in some trials become ill-conditioned. Most importantly, the bank of MAP estimators performs substantially better than its competitors, in terms of both

accuracy and consistency. This is attributed to the good initial estimates attained through the algebraic methods (see Section 4.2). TR-2010-0001 16
Page 18
20 40 60 80 100 120 140 160 180 200 0.2 0.4 0.6 0.8 1.2 1.4 1.6 1.8 Time Steps CPU time PF MAP bank (a) Sliding window size: 20, particles #: 2000 20 40 60 80 100 120 140 160 180 200 Time Steps CPU time PF MAP bank (b) Sliding window size: 50, particles #: 2000 20 40 60 80 100 120 140 160 180 200 Time Steps CPU time PF MAP bank (c) Sliding window size: 50, particles #: 5000 Figure 6: Comparison results of running time for the PF

and proposed bank of MAP estimation algorithm. Note that these results are obtained from one typical realization of the Monte Carlo simulations. Finally, Fig. 6 shows the comparison results of execution time for the PF and the proposed bank of MAP estimators. These results are obtained from a Core i5 CPU running in Matlab. We see from Fig. 6 that the proposed algorithm is computationally more expensive than the PF for the simulation setup from which the preceding results are attained (see Fig. 6(b)). However, we stress that the proposed bank of MAP estimators is resource-adaptive , i.e., its

computational cost can be controlled based on the available computational resource by adjusting the size of the sliding window as well as the pruning parameter. For instance, in this simulation, if available resource becomes stringent, we reduce the sliding window size to 20 time steps. As a consequence, the proposed algorithm becomes more ecient than the PF (see Fig. 6(a)). Additionally, it is known that the PF suers from the curse of dimensionality as well as particle depletion, and usually needs a large number of particles to increase its robustness [16]. We nd from

simulations that the PF can easily become more computational demanding than the proposed algorithm if moderately increasing the number of particles, e.g., instead using 5000 particles in this simulation (see Fig. 6(c)). 7 Conclusions In this paper, we have studied the estimator consistency issue of bearing-only tracking. When designing consistent nonlinear estimators, it is desirable for the estimators to possess an ability to track multimodal pdfs. However, this is not the the case for many existing estimators (e.g., the EKF, and the MAP esti- mator). In this paper, we have introduced a

general estimation framework, a bank of MAP estimators, that simultaneously allows tracking multiple modes of the pdf, and reduces linearlization errors through relinearization of past states. We have then applied it to the problem of bearing-only tracking. Due to its generally computational intractability, we have employed a relaxation scheme that keeps past state estimates constant and incrementally solves a one-step minimization problem for current state estimate at every time step. This minimization is solved analytically using algebraic geometry methods. The analytically computed local

minima are then used to nd accurate initial values for the bank of MAP estimators, so that it can focus the available resources on tracking most probable hypotheses of the target trajectory. Additionally, to reduce the computational complexity of the proposed algorithm, we have employed an eective pruning scheme and the process of marginalization of old states. Simulation results have shown that the proposed algorithm signicantly outperforms the the standard EKF, the batch MAP estimator, as well as the PF, in terms of both accuracy and consistency. We omit the

comparison results of computational complexity between the proposed approach and the standard MAP estimator as well as the EKF, since these results are conceivable, i.e., the proposed bank of MAP estimators is typically more expensive. TR-2010-0001 17
Page 19
References [1] S. Thrun, W. Burgard, and D. Fox, Probabilistic robotics . Cambridge, MA: The MIT Press, 2005. [2] S. S. Blackman and R. F. Popoli, Design and Analysis of Modern Tracking Systems . Artech House, 1999. [3] Y. Bar-Shalom, X. Li, and T. Kirubarajan, Estimation with applications to tracking and navigation John Wiley &

Sons, Inc., 2001. [4] T. Kronhamn, \Bearings-only target motion analysis based on a multihypothesis Kalman lter and adaptive ownship motion control," IEE Proceedings - Radar, Sonar and Navigation , vol. 145, no. 4, pp. 247 {252, Aug. 1998. [5] J. M. C. Clark, R. B. Vinter, and M. M. Yaqoob, \Shifted Rayleigh lter: a new algorithm for bearings- only tracking," IEEE Trans. Aerospace and Electronic Systems , vol. 43, no. 4, pp. 1373{1384, Oct. 2007. [6] S. Julier, J. Uhlmann, and H. F. Durrant-Whyte, \A new method for the nonlinear transformation of means and covariances in

lters and estimators," IEEE Trans. on Automatic Control , vol. 45, no. 3, pp. 477{482, Mar. 2000. [7] S. Kay, Fundamentals of Statistical Signal Processing, Vol. I - Estimation Theory . Prentice Hall, 1993. [8] M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, \A tutorial on particle lters for online nonlinear/non-Gaussian Bayesian tracking," IEEE Trans. on Signal Processing , vol. 50, no. 2, pp. 174{ 188, Feb. 2002. [9] D. Cox, J. Little, and D. OShea, Using Algebraic Geometry . Springer, 2005. [10] G. P. Huang, K. X. Zhou, N. Trawny, and S. I. Roumeliotis, \A bank of

MAP estimators for single-sensor range-only target tracking," in Proc. American Control Conference , Baltimore, MD, Jun.30{Jul.2, 2010, pp. 6974{6980. [11] A. Farina, \Target tracking with bearing-only measurements," Signal Processing , vol. 78, pp. 61{78, Oct. 1999. [12] V. Aidala and S. Hammel, \Utilization of modied polar coordinates for bearings-only tracking," IEEE Trans. on Automatic Control , vol. 28, no. 3, pp. 283{294, 1983. [13] D. Musicki, \Bearings only single-sensor target tracking using Gaussian mixtures," Automatica , vol. 45, no. 9, pp. 2088{2092, Sep. 2009.

[14] F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, R. Karlsson, and P.-J. Nordlund, \Particle lters for positioning, navigation and tracking," IEEE Trans. on Signal Processing , vol. 50, no. 2, pp. 425{437, Feb. 2002. [15] B. Ristic, S. Arulampalam, and N. Gordon, Beyond the Kalman Filter: Particle Filters for Tracking Applications . Artech House, 2004. [16] F. Gustafsson, \Particle lter theory and practice with positioning applications," Aerospace and Elec- tronic Systems Magazine, IEEE , vol. 25, no. 7, pp. 53 {82, 2010. [17] B. Triggs, P. McLauchlan, R. Hartley, and

Fitzgibbon, \Bundle adjustment { a modern synthesis," in Vision Algorithms: Theory and Practice . Springer Verlag, 2000, pp. 298{375. [18] I. Z. Emiris and A. Rege, \Monomial bases and polynomial system solving," in Proc. of the International Symposium on Symbolic and Algebraic Computation , Oxford, UK, Jul. 20{22, 1994, pp. 114{122. [19] G. H. Golub and C. F. V. Loan, Matrix Computations . The Johns Hopkins University Press, 1996. TR-2010-0001 18
Page 20
[20] H. Stark and J. W. Woods, Probability and Random Processes with Applications to Signal Processing Prentice Hall, 2001. [21]

A. H. Jazwinski, Stochastic Processes and Filtering Theory . Academic Press, 1970. [22] B. M. Bell and F. W. Cathey, \The iterated Kalman lter update as a Gauss-Newton method," IEEE Trans. on Automatic Control , vol. 38, no. 2, pp. 294{297, Feb. 1993. [23] A. Edelman and H. Murakami, \Polynomial roots from companion matrix eigenvalues," Mathematics of Computation , vol. 64, no. 210, pp. 763{776, Apr. 1995. [24] D. P. Bertsekas, Nonlinear Programming . Athena Scientic, 1999. [25] Y. Jabri, The Mountain Pass Theorem: Variants, Generalizations and Some Applications . Cambridge

University Press, 2003. [26] R. O. Duda, P. E. Hart, and D. G. Stork, Pattern Classication . Wiley-Interscience, 2000. [27] E. D. Nerurkar, S. I. Roumeliotis, and A. Martinelli, \Distributed maximum a posteriori estimation for multi-robot cooperative localization," in Proc. IEEE International Conference on Robotics and Au- tomation , Kobe, Japan, May 12{17, 2009, pp. 1402{1409. [28] A. Doucet, N. de Freitas, and N. Gordon, Eds., Sequential Monte Carlo Methods in Practice . Springer, 2001. TR-2010-0001 19