Portal de Periódicos da CAPES

Trajectory Optimization for Multitarget Tracking Using Joint Probabilistic Data Association Filter

2019; American Institute of Aeronautics and Astronautics; Volume: 43; Issue: 1 Linguagem: Inglês

10.2514/1.g004249

1533-3884

Shaoming He, Hyo‐Sang Shin, Antonios Tsourdos,

Gaussian Processes and Bayesian Inference

Open AccessEngineering NotesTrajectory Optimization for Multitarget Tracking Using Joint Probabilistic Data Association FilterShaoming He, Hyo-Sang Shin and Antonios TsourdosShaoming HeCranfield University, Cranfield, England MK43 0AL, United Kingdom, Hyo-Sang ShinCranfield University, Cranfield, England MK43 0AL, United Kingdom and Antonios TsourdosCranfield University, Cranfield, England MK43 0AL, United KingdomPublished Online:20 Oct 2019https://doi.org/10.2514/1.G004249SectionsRead Now ToolsAdd to favoritesDownload citationTrack citations ShareShare onFacebookTwitterLinked InRedditEmail AboutI. IntroductionAirborne target tracking is a key enabling technology in many civilian and military applications, including situation awareness [1–3], vehicle monitoring [4,5], public surveillance [6], and traffic management [7]. However, reliable target tracking becomes challenging when dealing with highly complex multitarget tracking (MTT) problem. The objective of MTT is to simultaneously estimate the number of targets and their states. In MTT, except for the fact that the number of targets varies randomly in time, the received measurements are also subject to a certain degree of uncertainties: unknown source origin, miss detections, and false alarms. There are several elegant solutions available in the literature to address the measurement uncertainty problem in MTT: nearest neighbor (NN) filter [8], probabilistic data association (PDA) filter [9], joint probabilistic data association (JPDA) filter [10,11], and multiple hypothesis tracking (MHT) [12–14]. This Note adapts the JPDA filter as the baseline multitarget tracker because of its balance between estimation accuracy and computational cost.It is known that the estimation accuracy depends not only on the filter performance but also on the relative geometry between the unmanned aerial vehicle (UAV) and the targets [15–17]. For this reason, trajectory optimization or path planning that directly or indirectly minimizes the estimation error has been widely studied in the literature. Optimal UAV trajectories that minimize the estimation error are generated in [18–21] using numerical optimizations by maximizing the determinant of Fisher information matrix (FIM) over a finite time horizon. The rationale of using FIM for a cost function lies in that the inverse of FIM prescribes a lower bound, also known as posterior Cramer–Rao bound (PCRB), of the estimation error covariance of an unbiased filter [22]. By maximizing the approximate lower bound of FIM, an optimal variable deviated pursuit guidance algorithm that improves the cooperative estimation performance was proposed in [23] for a UAV rendezvous mission. Several trajectory optimization algorithms for spacecraft rendezvous were reported in [24–26] to improve the estimation quality. An algorithm for active localization of stationary targets using ground robots was suggested in [27,28] by leveraging the trace of error covariance matrix as the cost function.Although the aforementioned algorithms can bring significant benefits in target tracking, they are mainly limited to single target tracking (STT) scenarios. The aim of this Note is, therefore, to propose a new optimization framework for a fixed-wing UAV to find its optimal trajectory that improves the MTT estimation performance. In trajectory optimization, it is known that formulating a suitable cost function, or objective function, is of significant importance. The difficulty in formulating a simple yet pertinent cost function for MTT, however, naturally arises in the inevitable data association uncertainty and the random number of targets. To simplify the problem formulation, the authors in [4,29,30] leveraged the FIM, analogous to the approach used in STT, to formulate the cost function in trajectory optimization for MTT: the objective function is defined as the summation of all FIMs from all existing targets. This simple cost function, however, ignores the inherent data association uncertainty in MTT and thus cannot really reflect the estimation performance of the tracker. Instead of using FIM, the Rényi information divergence between the posterior and prior distributions was used in [31] for sensor scheduling to track multiple targets. However, calculating the information-theoretic Rényi information divergence for a Gaussian mixture requires computationally expensive Monte-Carlo techniques. The authors in [32] derived the MTT PCRB by taking into account data association uncertainty. Although this performance metric could be an ideal candidate objective function in trajectory optimization for MTT, calculating the MTT PCRB requires computationally expensive Monte-Carlo integration. Therefore, these optimization frameworks might not be suitable for online implementation.Unlike STT, the objective of an MTT algorithm is to simultaneously estimate the number of targets and their states. The cost function, therefore, should be a balance between cardinality estimation performance and target localization performance. Taking this factor into account, this Note formulates an analytical cost function for MTT by leveraging the properties of the JPDA filter. The cost function developed is a weighted sum of multitarget estimation variance and cardinality estimation variance. By minimizing the cost function, we can therefore increase the confidence level of JPDA filter, thus indirectly improving the estimation performance. As discussed later, the multitarget estimation variance can also be viewed as the data association uncertainty. This means that the proposed cost function also provides the possibility to improve the quality of data association. Incorporating the objective function formulated with physical constraints, for example, turning rate limit, a constrained trajectory optimization framework for MTT is proposed. Because the objective function is nonconvex in general, an approximate solution is obtained by the control input discretization. The resultant solution, given as heading angle input command, is simple to be implemented in practice through an onboard heading tracking control system.Realistic scenarios are simulated to illustrate and evaluate the performance of the proposed algorithm, and the results clearly demonstrate that the proposed approach can significantly improve the MTT estimation performance, especially when the detection probability is time varying.The rest of the Note is organized as follows. Section II presents some mathematical models used in this study. Section III gives a brief review of JPDA filter, and Sec. IV provides the details of the proposed cost function, followed by the trajectory optimization solution shown in Sec. V. Finally, some numerical simulations are demonstrated.II. Mathematical ModelsThis section provides necessary preliminaries of several important mathematical models to facilitate the analysis in the following sections.A. UAV Kinematics ModelThis work assumes that the UAV is equipped with a high-performance low-level flight control system that provides roll, pitch, and yaw stability as well as velocity tracking, heading, and altitude hold functions. This study aims to design guidance input, for example, heading angle command, and feed this to the low-level controller for multiple targets localization and is constrained to the two-dimensional (2-D) motions. The UAV's kinematics in a 2-D environment is given by p˙xu=Vucosψup˙yu=Vusinψu(1)where (pxu,pyu) stands for the UAV position in an inertial coordinate. ψu is the UAV heading angle, and Vu denotes the UAV speed. For simplicity, the following general assumption is made.Assumption 1:The speed of UAV is assumed to be constant. Note that the UAV speed is often predefined for operational reasons, for example, endurance and mission objectives.In practice, the heading change of a fixed-wing UAV between two consecutive time steps is constrained due to physical turning rate limitation as |ψu,k−ψu,k−1|≤ψmax≜ψ˙maxTs(2)where ψu,k represents the heading angle at time step k, ψ˙max the maximum permissible turning rate of the UAV, and Ts the sampling time.B. Multitarget State and Measurement ModelSuppose that there are Nk targets and Mk sensor measurements at scan k. A multitarget state Xk and a multitarget measurement Zk are then defined as Xk={xk1,…,xkNk}Zk={zk0,zk1,…,zkMk}(3)where xki denotes the ith target at scan k, zkj(j≠0) the jth measurement received at scan k, and zk0 the dummy measurement for convenient representation of miss detection.Consider the following dynamical system of target: xki=fk−1i(xk−1i)+wk−1izki=hki(xki)+vki(4)where xki∈Rn and zki∈Rm denote the system state and the corresponding measurement of the ith target at time step k. The nonlinear functions fki(xki) and hki(xki) correspond to the system state evolution and measurement equations, respectively. The signals wki and vki are process noise and measurement noise, which are assumed to be zero-mean Gaussian with covariances Qki and Rki. For convenience, we make the following general assumptions, which are widely accepted in MTT problems.Assumption 2:Each target can generate at most one measurement, and each measurement can originate from at most one target. Each target-generated measurement is independent of each other and is detected with probability PD. Notice that the detection probability, by default, depends on the relative geometry between the UAV and the target and therefore is usually a time-varying variable.Assumption 3:The clutter distribution is assumed to be unknown a priori and is thus considered as Poisson distribution. Clutters or false alarms are modeled by a local Poisson point process with intensity λFA=NFA/Vs, with NFA being the average number of false alarms received at each scan and Vs being the sensor volume.C. Target Existence ModelIn MTT, there is no prior information on the source of received measurements. That is, each measurement may be spurious, for example, false alarm, or from one existing/new target. For this reason, both true tracks, representing real targets, and false tracks are generated and updated. The number of targets at time instant k is also a random variable in an MTT problem: one target might suddenly disappear or appear in the sensor's field of view. Because of these facts, a track confirmation and deletion logic is essential in MTT to confirm the majority of true tracks and terminates most false tracks.In this Note, each track is confirmed or terminated using thresholding based on target existence probability ρki≜p(χki|Zk), with χki being the event of existence of the ith target at scan k. If ρki is larger than a upper threshold α1, then the ith track is confirmed; once ρki is below a certain lower bound α2, the ith track is immediately deleted. For the case where α2≤ρki≤α1, the ith track is tentative and requires more information to confirm or delete. The time evolution of χki can be formulated by p(χki|Zk−1)=PSp(χk−1i|Zk−1)(5)where PS denotes the surviving probability. For target birth, the following general assumption is used in this Note.Assumption 4:The number of new targets is assumed to be unknown a priori and is thus considered as Poisson distribution. New targets are modeled by a local Poisson point process with intensity λB=NB/Vs, with NB being the average number of new targets.Remark 1:Note that an MTT algorithm creates a new track for each measurement received at every scan. This means that the number of tracks grows exponentially as time goes. To maintain computational feasibility, we use a thresholding-based track deletion logic to remove unreliable tracks. Therefore, the tracks kept at each scan include confirmed tracks and tentative tracks.III. Joint Probabilistic Data Association FilterIn a general MTT mission, the relationship between targets and measurements is unknown and the number of targets is also a random variable. The challenge is that each target can appear and disappear at any place and any time. Data association is a widely accepted and plausible solution to resolve the problem of measurement origin uncertainty. This technique discerns target-generated measurements from clutters and finds the mappings between targets and measurements, and therefore is the key in MTT.JPDA is a well-established single-scan data association approach based on probabilistic reasoning [10,11]. This approach associates the measurements to the targets under the assumption that the relationship between targets and measurements satisfies: 1) each measurement (except for the dummy one) is assigned to at most one target, and 2) each target is uniquely assigned to a measurement. Based on the assumption, the approach uses the joint association hypothesis that is denoted as Θk={θki}, i∈{1,2,…,Nk|k−1+Mk}. The joint association hypothesis and corresponding probability measures play a key role in the data association filter. For each pre-existed target i∈{1,2,…,Nk|k−1}, define θki∈{0,1,…,Mk} as the association hypothesis, where Nk|k−1 stands for the predicted number of targets at scan k. As we have no information on target birth/death at scan k before receiving the measurements, Nk|k−1 is determined as Nk|k−1=Nk−1|k−1. The single association event θki=j refers to the fact that the jth measurement originates from the ith target and θki=0 represents miss detection. We create a new track for each measurement j∈{1,2,…,Mk} at scan k, and the association event for these new targets are defined by θkNk|k−1+j∈{Nk|k−1+1,…,Nk|k−1+Mk}. That is, if target Nk|k−1+j is associated with the jth measurement, then θkNk|k−1+j=Nk|k−1+j. Under the assumption that each single association event is independent, the minimum-mean-squared-error (MMSE) estimate of each target is given by p(xki|χki,Zk)=∑θkip(xki|θki,χki,Zk)p(θki|χki,Zk)(6)In practice, propagation of mixture is computationally intractable due to the explosion of mixture terms. JPDA approximates mixture (6) by a single probability density function based on simple moment-preserving approach. More specifically, the state correction xk|ki of the ith target and its corresponding covariance Pk|ki are obtained as xk|ki=∑j=0Mkβjixk|ki,jPk|ki=∑j=0Mkβji{Pk|ki,j+(xk|ki,j−xk|ki)(xk|ki,j−xk|ki)T}(7)where xk|ki,j denotes the target estimation by associating the jth measurement to the ith target, Pk|ki,j the corresponding covariance, and βji=p(θki=j|χki,Zk) the existence-conditioned marginal association probability that the jth measurement is associated with the ith target. Note that the hypothesis-conditioned estimation (xk|ki,j,Pk|ki,j) can be calculated with a standard Kalman filter algorithm. Notice that if an existing target is miss detected at one scan, its posterior estimation is constrained as its corresponding prediction, that is, (xk|ki,j,Pk|ki,j)=(xk|k−1i,Pk|k−1i), in track update.According to Bayesian theory, the existence-conditioned marginal association probability p(θki|χki,Zk) is determined by p(θki|χki,Zk)=p(θki,χki|Zk)p(χki|Zk)=p(χki|θki,Zk)p(θki|Zk)p(χki|Zk)(8)where the hypothesis-conditioned existence probability p(χki|θki,Zk) is determined as p(χki|θki,Zk)∝{p(χki|Zk−1)(1−PD)1−p(χki|Zk−1)+p(χki|Zk−1)(1−PD),θki=01,θki=jPDλBp(zkj|xb)λFA+PDλBp(zkj|xb),θkNk−1+j=Nk−1+j(9)and the posterior existence probability is given by p(χki|Zk)=∑θkip(θki,χki|Zk)(10)with p(θki,χki|Zk)=p(χki|θki,Zk)p(θk|Zk)(11)According to the law of total probability, p(θki|Zk) can be theoretically calculated by enumerating all possible joint hypotheses as p(θki=j|Zk)=∑θki(∈Θk)=jp(Θk|Zk)(12)where the posterior distribution of the joint association event p(Θk|Zk) is given by p(Θk|Zk)∝[∏i∈[Nk|k−1],θki=01−PDp(χki|Zk−1)]×[∏i∈[Nk|k−1],θki=jPDp(χki|Zk−1)p(zkj|xk|k−1i)]×[∏θkNk|k−1+j=Nk|k−1+jλFA+PDλBp(zkj|xb)](13)where xb denotes the candidate states of new born targets.In summary, each track in JPDA is updated through Eqs. (7–12). The outputs of JPDA at each time instant are the posterior existence probability p(χki|Zk) and posterior state estimate (xk|ki,Pk|kj). Note that the number of targets can be easily estimated by counting the confirmed tracks. Therefore, JPDA provides a complete framework for MTT and track management.IV. Cost Function FormulationFor UAV trajectory optimization, formulating a pertinent cost function is of paramount importance. JPDA filter provides sequential estimation of the target states as well as the number of targets. Because both target states and number of targets are typically time varying in MTT, the cost function should, therefore, provide an overall evaluation to quantify the performance of both cardinality and state estimations. The optimal subpattern assignment (OSPA) distance, proposed in [33], provides an overall evaluation of cardinality and position estimation performance for MTT. However, calculating this metric requires the knowledge of ground truth, which is obviously not available to the UAV. For this reason, this Note will formulate an alternative cost function, which is defined based on the average expected estimation variances of multitarget state and target number, for UAV trajectory optimization. More specifically, the proposed cost function J is defined as J=ωJs+(1−ω)Jn(14)where Js is related to the one-step expected estimation variance of multitarget state; Jn quantifies the one-step expected estimation variance of target number; and ω∈[0,1] is a weighting factor. Note that, in order to enforce the balance between multitarget state estimation variance and target number estimation variance in cost function J, both Js and Jn are normalized to be dimensionless.The reason behind choosing this cost function is clear: minimizing the estimation variance can increase the confidence of the tracker, thus indirectly improving the estimation accuracy. Obviously, increasing the value of ω enforces more penalty on the performance of multitarget state estimation. It is clear that the cost function J at time instant k is a function of UAV's heading angle ψu,k, and calculating the cost function J requires running JPDA to get the predicted or expected performance. To this end, we manually generate one-step predicted measurement set Z¯k using currently confirmed targets and available environmental information, that is, PD, λFA, and λB. These virtual measurements are then used to run JPDA for computing the cost function.A. Calculation of JsIn target tracking or localization, the primary interest is to accurately estimate the positions of targets. For this reason, we calculate Js based on multitarget position estimation variance. From Eq. (6), we know that the original MMSE estimate of the ith target is given by a Gaussian mixture ∑j=0MkβjiN(⋅;xk|ki,j,Pk|ki,j) due to the nature of data association uncertainty. Let us define pki,j=[px,ki,j,py,ki,j]T as the position estimation vector of the ith target extracted from xk|ki,j. Then, the variance of pki,j of the ith target can be readily calculated as Var[pki,j]=[E[(px,ki,j−E[px,ki,j])2]E[(px,ki,j−E[px,ki,j])(py,ki,j−E[py,ki,j])]E[(py,ki,j−E[py,ki,j])(px,ki,j−E[px,ki,j])]E[(py,ki,j−E[py,ki,j])2]]=[Var[px,ki,j]E[(px,ki,j−E[px,ki,j])(py,ki,j−E[py,ki,j])]E[(py,ki,j−E[py,ki,j])(px,ki,j−E[px,ki,j])]Var[py,ki,j]](15)where Var[px,ki,j]=E[(px,ki,j−E[px,ki,j])2]=E[(px,ki,j)2]−E[px,ki,j]2=∑j=0Mkβji(px,ki,j)2−(∑j=0Mkβji(px,ki,j))2(16)and Var[py,ki,j]=∑j=0Mkβji(py,ki,j)2−(∑j=0Mkβji(py,ki,j))2(17)As it would be beneficial to consider a scalar objective function instead of a matrix form for the trajectory optimization, we leverage the trace of Var[pki,j], denoted as σpi, to quantify the position estimation uncertainty of the ith target as σpi=Trace(Var[pki,j])=Var[px,ki,j]+Var[py,ki,j](18)Note that JPDA approximates the posterior Gaussian mixture ∑j=0MkβjiN(⋅;xk|ki,j,Pk|ki,j) by a single Gaussian using moment matching to reduce the computational burden. The approximation error of this simple moment-preserving approach increases with higher modality of the Gaussian mixture. Because the modality of Gaussian mixture ∑j=0MkβjiN(⋅;xk|ki,j,Pk|ki,j) can be characterized by the variances of xk|ki,j, minimizing Var[pki,j] provides the possibility to improve the quality of data association in JPDA. This is clearly helpful in improving the multitarget state estimation performance. From Eqs. (16) and (17), it is easy to verify that the position estimation variances of the ith target are minimized as Var[px,ki,j]=0 and Var[py,ki,j]=0 if and only if there exists one j′ such that βj′i=1 and βji=0, ∀j≠j′. This obviously coincides with the ideal case with no data association uncertainty. Furthermore, one can also imply that both Var[px,ki,j] and Var[py,ki,j] take their maximum values once all candidate association pairs of the ith target are equally possible, that is, βji=1/(Mk+1). This condition corresponds to the largest data association uncertainty and the highest modality of Gaussian mixture ∑j=0MkβjiN(⋅;xk|ki,j,Pk|ki,j), in which all Gaussian terms are equally important. The maximum values of Var[px,ki,j] and Var[py,ki,j] are, respectively, determined as max{Var[px,ki,j]}=Mk(Mk+1)2∑j=0Mk(px,ki,j)2max{Var[py,ki,j]}=Mk(Mk+1)2∑j=0Mk(py,ki,j)2(19)For all confirmed tracks, the position estimation variance is normalized to be dimensionless as σp¯i=σpimax{σpi}(20)where the denominator finds the maximum individual variance from all σpi.Notice that each track has its corresponding existence probability ρki. Therefore, it is natural to enforce more penalty on the track that has higher existence probability. With this in mind, Js is defined as a weighted arithmetic mean of σp¯i as Js=∑i=1Nk|kρkiσp¯i∑i=1Nk|kρki(21)It follows from Eq. (21) that tracks with a high existence probability contribute more to the weighted mean than tracks with a low existence probability. Therefore, minimizing Js also provides the possibility to improve the quality of track management: discriminating real targets from false alarms.Remark 2:Although Pk|ki,j characterizes the estimation accuracy of the ith target conditioned on the jth measurement, leveraging ∑j=0Mkβjitrace(Pk|ki,j) as the performance measure for the ith target in trajectory optimization is not meaningful for MTT, because there only exists at most one measurement that comes from the ith target in reality. Instead, minimizing σpi, shown in Eq. (18), can reduce the multimodality of Gaussian mixture ∑j=0MkβjiN(⋅;xk|ki,j,Pk|ki,j), thus improving data association quality.B. Calculation of JnAn MTT algorithm also needs to estimate the number of targets Nk. To this end, Jn will be defined based on the variance of cardinality estimation, which directly indicates the cardinality estimation accuracy. Recall that the outputs of JPDA filter are multitarget state estimates xk|ki as well as their corresponding existence probabilities ρki and denote Nk|ki as the cardinality estimate of the ith track. Then, one can easily verify that Nk|ki satisfies a Bernoulli distribution as p(Nk|ki)={ρkiif Nk|ki=1,1−ρkiif Nk|ki=0(22)From the property of Bernoulli distribution, one can conclude that the expectation and variance of Nk|ki are, respectively, given by E[Nk|ki]=ρki(23)Var[Nk|ki]=E[(Nk|ki)2]−E[Nk|ki]2=ρki(1−ρki)(24)Notice that, in JPDA filter derivations, the estimations of all tracks are assumed to be independent. Then, according to Bienayme formula, the variance of target number estimation Var[Nk|k] can be readily obtained as Var[Nk|k]=Var[∑i=1Nk|kNk|ki]=∑i=1Nk|k(Var[Nk|ki])=∑i=1Nk|kρki(1−ρki)(25)Clearly, Var[Nk|k] achieves the minimum value zero at either ρki=1 or ρki=0, which corresponds to target always being existence or nonexistence. When ρki=0.5, Var[Nk|k] takes its maximum value Nk|k/4. This means that, when the posterior target existence probability equals 0.5, JPDA filter has the least confidence in its cardinality estimation, leading to the increase of cardinality estimation error.To provide a linear combination of Js and Jn in cost function J, we define Jn as the normalized version of Var[Nk|k] as Jn=∑i=1Nk|kρki(1−ρki)max{ρki(1−ρki)}(26)where the denominator finds the maximum individual variance from all Var[Nk|ki].From the aforementioned derivations, we know that the cardinality estimation performance related term Jn depends only on existence probability, whereas the multitarget state estimation performance related term Js depends on both state estimates and existence probability.V. Trajectory Optimization SolutionThe aim of the trajectory optimization is to determine UAV's optimal heading angle that minimizes the cost function (14) so as to indirectly increase the estimation performance of JPDA filter. To accomplish this goal, a discrete-time-constrained trajectory optimization problem is formulated, which is denoted as CTO1. CTO1: Findψu,k*=minψu,kJ(ψu,k)(27)subject to |ψu,k−ψu,k−1|≤ψ˙maxTs(28)Constraint (28) corresponds to the physical limit of turning rate, as discussed in Sec. II.A. Note that although the proposed problem considers only one-step-ahead optimization, the optimization problem formulated can be easily extended to a multistep-ahead optimization framework at the sacrifice of computational cost.Because the objective function described by Eq. (14) is in general nonconvex, the constrained trajectory optimization problem CTO1 is generally not a convex program. As the operational environment becomes more complicated, for example, the number of targets increases, CTO1 becomes more complicated to be solved. This implies that finding the exact solution of CTO1 in a polynomial time is not likely feasible. Therefore, this Note attempts to find an approximate solution of CTO1 to mitigate the computation issue resulting from the nonconvexity. By approximation, it is expected that the optimal solution of the approximated problem can be obtained in a matter of a few seconds. The approximated constrained trajectory optimization problem, denoted as CTO2, is: CTO2: Findψu,k*=minψu,k∈ΨJ(ψu,k)(29)subject to Ψ={ψlow,ψlow+Δψ,ψlow+2Δψ,…,ψlow+(L−1)Δψ}(30)where ψlow=ψu,k−1−ψ˙maxTs(31)Δψ=2ψ˙maxTsL(32)As can be noticed, CTO2 is discretized to reduce computational load in finding the solution of the trajectory optimization problem. By defining an admissible heading angle command set Ψ, CTO2 is set to find the optimal commands from a set of L discretized heading angles. Note that, with the definition of the feasible solution set Ψ, the obtained heading angle command automatically satisfies constraint (28). Note that this approximation strategy is also accepted to relax computational burden in other well-known algorithms such as the methods proposed in [34,35].The proposed algorithm of trajectory optimization for MTT using JPDA filter is summarized in Algorithm 1.Remark 3:Because the number of feasible heading angle commands is constrained to be finite, the exact solution of CTO2 (or the approximate solution of CTO1) can be easily found through exhaustive search. Therefore, as the size of Ψ increases, the solution of CTO2 is expected to become closer to the solution of CTO1 at the expense of increased computational load.VI. Numerical SimulationsIn this section, the effectiveness of the proposed trajectory optimization algorithm is demonstrated through Monte-Carlo simulations in a cluttered environment. In addition, this section also conducts the performance comparison of the proposed algorithm against nonoptimized trajectories.A. Simulation SetupOur experiments explore a scenario that 1 UAV tracks 10 moving targets with different birth times. The UAV is initially located at (0 m,−3000 m) with heading angle ψu,1=90°. The velocity of the UAV is Vu=40 m/s and the heading angle is constrained by maximum permissible turning rate ψ˙max=0.1415 rad/s. This corresponds to a maximum bank angle around 30°. For simplicity, the well-known constant velocity model is used as the transition model fki(xki) as xki=Fkxki+wk−1i(33)with Fk=[I2TsI202I2](34)where the notation 02 denotes a 2×2 zero matrix and I2 stands for a 2×2 identity matrix. The sampling time is given by Ts=1 s. The covariance matrix Qk of the process noise is defined as [36] Qk=σv2[Ts44I2Ts32I2Ts32I2Ts2I2](35)where the standard deviation of the process noise is given by σv=15 m/s2.The UAV is equipped with an active sensor, which provides range as well as bearing measurements. Therefore, each target-generated measurement zki can be modeled by zki=[(px,ki−px,ku)2+(py,ki−py,ku)2arctan(py,ki−py,kupx,ki−px,ku)]+vk(36)where vk∼N(⋅;0,Rk) is the Gaussian measurement noise with Rk=diag(σr2,σθ2), σr=5 m, and σθ=1°. To accommodate the nonlinear measurements, the well-known EKF is used in JPDA filter for measurement update of each target.The average number of false alarms at each scan is set as NFA=20. Gating is performed with a threshold such that the gating probability is PG=0.999 to reduce the computation burden of JPDA. The surviving probability for propagating target existence probability is set as PS=0.99. A tentative track is confirmed if the existence probability satisfies p(χki|Zk)≥0.8 and a confirmed track is deleted immediately once p(χki|Zk)≤0.1. The number of Monte-Carlo runs is set as 500 for all tested cases. Figure 1a shows a snapshot of one sample of the considered scenario, where the dashed color lines are target ground truth trajectories, and the solid red line is the UAV trajectory obtained by the proposed algorithm. The time histories of target ground truth trajectories and estimated trajectories are presented in Fig. 1b.Fig. 1 A snapshot of one sample of the considered scenario. a) Two-dimensional trajectories of the UAV and targets. The dashed color lines are target ground truth trajectories, and the solid red line is the UAV trajectory obtained by the proposed algorithm. b) Time histories of target ground truth trajectories and estimated trajectories.B. Performance MetricThe OSPA distance metric [33] is considered here for overall evaluation of performance, namely, cardinality and position estimation errors. Let X and Y be the position estimation set and