1. Introduction
In the last decades, target motion analysis (TMA) has become an increasingly popular research field, and in the literature, several approaches have been developed, such as batch processing frameworks [
1,
2,
3,
4] and recursive ones [
3,
5,
6,
7,
8]. The aim of TMA is to estimate the state of a target (usually position and velocity) from noise-corrupted measurements collected by an observer [
9]. The TMA problem presents several challenges, mainly due to the nonlinear relationship between the measurements and target state. Another challenge is that the observer must outmaneuver the target in order to make the target state observable [
10]. For instance, to track a target with constant velocity, the observer platform must change its speed or course. Otherwise, there exist other target trajectories that produce the same sequence of noise-free bearing angles [
3].
Among other approaches, bearing-only target tracking [
11,
12,
13] represents an increasingly popular topic, with application scenarios ranging from underwater tracking [
14,
15] to cooperative tracking for multiagent systems [
15,
16,
17,
18].
Other relevant approaches in the literature include, among other works: applications to sensor network localization [
19]; algorithms based on direction-of-arrival measurements, modeled by von Mises–Fisher distributions [
20]; pseudolinear estimators for 3D target motion analysis by a single moving ownship collecting azimuth and elevation angle measurements [
21]; a methodology based on a bank of batch maximum a posteriori (MAP) estimators as a general estimation framework that provides the relinearization of the entire state trajectory, multihypothesis tracking and an efficient hypothesis generation scheme [
22]; an approach based on Newton–Raphson methods and Particle Swarm Optimization [
23]; a methodology to combine target motion compensation and track-before-detect methods within passive radar based on global navigation satellite systems (GNSS) for the detection of maritime targets [
24]; TMA from cosines of conical angles acquired by a towed array [
25]; and a new pseudolinear filter for bearings-only tracking without the requirement of bias compensation [
26].
Notice that, in the literature, some approaches have been developed where the availability of road or traffic information is used to track a moving target. In particular, in [
27], the target is assumed to move in a a road network, and the tracking is performed via an airborne sensor that exploits knowledge on the network; in [
28], a similar setting is considered, and a Bayesian approach is adopted; in [
29], a particle filter is developed in order to track multiple vehicles on multi-lane roads based on a microscopic traffic flow model. However, such approaches require one to rely on a large deal of fine-grained information (e.g., the structure of the road network) and can only be applied to scenarios involving roads. However, in many cases, especially considering a maritime context, only coarse-grained information is available: for instance, the environment might contain physical obstacles or deterring entities such as warships that discourage the target from passing nearby, or there might be rough evidence of the presence of a target in a given zone (e.g., due to a witness or to cheap range-free sensors able to only detect the presence of a target in a given zone). In this view, relying on such a coarse-grained information could help improve the target’s trajectory estimate, also in contexts where road network information cannot be leveraged upon without requiring huge computational resources. This has been demonstrated, for instance, in [
30] where such information is used in the framework of network localization to overcome localization ambiguities.
This is the aim of this paper. In particular, this paper considers a scenario where additional information is available to the ownship in charge of estimating the target’s trajectory; specifically, the ownship is aware that the trajectory of the target lies in the intersection of some sets and is not contained in the union of some other sets. This additional information is exploited by developing a constrained MLE problem and an approach for the selection of the ownship’s trajectory mimicking the Artificial Potential Fields technique [
31,
32], which is typically used by mobile robots to aim at a goal (in this case, the region where the target is assumed to be) while avoiding obstacles (i.e., the region that is assumed not to intersect the target’s trajectory). Moreover, from a theoretical standpoint, the CRLB on the estimation covariance matrix is characterized in the case of MLE problems with inequality constraints; this is performed by extending the approach in [
33,
34], where equality constraints where discussed, via the cast of inequality constraints into nonsmooth equality ones and by the adoption of generalized Jacobian matrices [
35], which are set-valued on a zero-measure set where the derivative of the resulting nonsmooth function is not defined.
The paper is complemented by an experimental analysis showing the effectiveness of the proposed approach.
To summarize, the main contributions of the paper are as follows:
We develop a novel MLE approach to carry out batch target-tracking estimation based on noisy bearing-only measurements, which incorporates as inequality constraints additional information in terms of sets where the target’s trajectory is assumed to be contained and other sets which have empty intersection with the target’s trajectory;
We characterize the CRLB associated to the constrained problem by considering a generalized set-valued Jacobian matrix of the constraints function and by resorting to nonsmooth theory;
We provide a heuristic way to select the ownship’s trajectory based on the although coarse-grained available information regarding the target’s trajectory.
2. Materials and Methods
The aim of this section is to present the main algorithms, tools and derivations that are used in the paper. Specifically, the section is organized as follows: in
Section 2.1, we present our problem statement; then, we discuss maximum likelihood estimation problems (in
Section 2.2, we review the unconstrained case, while in
Section 2.3 we develop the proposed MLE approach with additional inequality constraints);
Section 2.4 is devoted to addressing the computational aspects related to the approximated solution of the above constrained and unconstrained MLE problems;
Section 2.5 and
Section 2.6 address, respectively, the characterization of the CRLB in the unconstrained and constrained case; finally,
Section 2.7 discusses a heuristic approach to choosing the ownship’s direction based on the available information.
2.1. Problem Statement
Let us consider a scenario where a target moves in a linear motion on a plane; in particular, considering a discrete-time sampling, let us assume that the target moves according to the following equations:
with
T being the sampling time. Moreover, let us consider an ownship platform aiming to estimate the parameter vector
based on a batch of measurements, sampled at uniform discrete-time instants
during the ownship motion and evaluated over the time interval
.
In more detail, we assume that the ownship attempts to sense the following
nominal measurement (e.g., see [
36]):
where
are the coordinates of the ownship at time
along the x and y axes, respectively. However, we consider a scenario where the ownship is actually provided with noisy measurements with the following structure:
where the terms
are independent and identically distributed Gaussian noises with zero-mean and variance
.
Further to that, let us assume that additional information is available; specifically, let us assume that the ownship is aware that, during the considered time frame
, the position
of the target is confined in a region
of the plane defined as follows:
where the sets
are convex, and their intersection is nonempty; moreover, the ownship is aware that the position
lies outside a region
of the plane defined as follows:
where the sets
are convex.
The aim of this paper is to investigate how this additional information influences the estimation of and how the ownship can leverage on this additional information to select a trajectory that improves the estimation performance.
2.2. Maximum Likelihood Estimation without Additional Information
Let us discuss how to estimate the parameter vector
via the maximum likelihood estimate (MLE) technique (see, for instance [
37], p. 182). The MLE for a vector parameter
is defined to be the value
that maximizes the likelihood function
over the allowable domain of
. In what follows, where understood, we abbreviate the notation by writing
. When
is differentiable, we have that the MLE
satisfies
It is worthwhile to mention that the solution of the above equation is unique in this case and is theoretically asymptotically unbiased. For our case, we have the following expression for the likelihood function [
3]:
Notably, the maximization problem at hand can be formulated as
Let us define
. The above problem can be equivalently expressed as
which, by simple computations is equivalent to solving [
3]
Notably, Equation (
10) is recognized to be the classical least squares (LS) solution.
2.3. Maximum Likelihood Estimation with Additional Information
Let us now extend the above MLE framework in order to account for the additional intelligence available to the ownship.
In particular, we notice that, by Equations (
1) and (
2), it holds that
with
Therefore, by plugging the above expression in Equation (
4), we have that
if and only if
while
if and only if
In the following section, we assume that the constraints in the form
can be equivalently expressed as
for some
and for some differentiable function
. In this view, the above unconstrained MLE problem can be extended as follows:
2.4. Computational Approach to Solve MLE Problems
Notice that, as remarked in [
38], the MLE for bearings-only target motion analysis does not admit a closed-from solution and must be implemented iteratively, considering an initialization close to the true solution to avoid divergence. In particular, we point out that the above MLE minimization problems (both in the unconstrained and constrained fashion) are not, in general, convex, thus calling for approximated solution schemes that typically aim to find a good local optimum. In this paper, we resort to the MIDACO-SOLVER optimization software, which implements an extension of the evolutionary ant colony optimization meta-heuristic [
39] and which has been developed especially for highly nonlinear real-world applications. See [
40] or [
41] for a focus of the performance of MIDACO software with respect to the state of the art. Notably, MIDACO-SOLVER allows one to evaluate the satisfaction of the constraints and the objective function from an algorithmic standpoint, thus allowing one to also tackle the problems that are not easily expressed in a closed form nor easily solved by traditional solvers. Note that the suggested strategy is independent of a particular solver, but the nonconvex nature of the optimization problem suggests an evolutionary approach, such as genetic algorithms [
42].
2.5. CRLB of the Estimate in the Unconstrained Case
Let us now discuss a useful metric that represents a lower bound on the covariance matrix associated to the MLE estimation process. Specifically, in this subsection, we consider the unconstrained case, while the constrained one is discussed in the next subsection.
In particular, consider the problem of evaluating the CRLB for the estimated vector
of target parameters (see, for instance [
37], p.44). In particular, it is well known that the covariance matrix is bounded by the inverse of the Fisher information matrix (FIM)
J, i.e., it holds that
where
Note that the expectations in the above equation are taken with respect to
; moreover, the derivatives in
are evaluated at the true value of
(i.e.,
) or, alternatively, at the estimated value
(i.e.,
) if the true value is unknown. Let us now present a more direct expression of the FIM. To this end, let us express the gradient of
with respect to
as [
3]
then, we have that [
3]
where in the first equation, we used the fact that the measures are independent in order to neglect mixed terms, while the last equation follows from the consideration that
Notice that, in the
Appendix A, we provide the analytical expression of the entries of
.
2.6. CRLB for Constrained MLE
As demonstrated in [
33] (see also [
34]), assuming
, when the MLE problem has an equality constraint in the form
the CRLB can be computed starting from the unconstrained case, according to the following equation
where
is the
Jacobian matrix of the constraint function
, evaluated at
, i.e.,
Let us now extend this method to the case of inequality constraints. To this end, consider a constraint in the form of
where
is differentiable. We point out that the inequality constraint can equivalently be expressed in the form of an equality constraint using
where max is intended component-wise. Notably, the max function is globally Lipschitz (e.g., see [
43]); hence, if
is differentiable everywhere, we have that
is differentiable for all
, where
is a zero-measure set in the form
In this view, a natural extension is to resort to the
generalized Jacobian of
. Specifically, it follows from [
35] that the generalized Jacobian
of
is defined as
with
being the convex closure,
the classical Jacobian whenever it exists, and
the set of measure zero where
is not defined. In other words,
is in general set-valued at the points where the Jacobian is not defined and
when the Jacobian is defined. Consequently, in the case of inequality constraints, the CRLB is also, in general, set-valued, and it holds that
Notice that, in the event that coincides with a point in the zero-measure set , any metric based on the CRLB (e.g., variance for a specific parameter, norm of the matrix, etc.) is computed considering the worst case over the elements in the set .
2.7. Ownship Trajectory Selection Based on Artificial Potential Fields
The availability of additional information (i.e., knowledge on
and
) can be leveraged by the ownship in order to select a trajectory that allows one to improve the accuracy of the estimate, i.e., by moving along a direction that mediates between the attempt to get closer to
and the will to avoid
. In this paper, we borrow some of the key concepts of the so-called artificial potential fields (APF) technique [
31,
32] in order to accomplish this task. Within the APF approach, a robot has to navigate in a space toward a goal while avoiding one or more obstacles; this is achieved by associating a repulsive potential field to each obstacle and an attractive potential field to the goal so that, depending on the robot’s position, the robot moves in the direction of the force that corresponds to the antigradient of the overall potential field.
For the application at hand in this paper, we consider a repulsive potential field to be associated with each set that the target is assumed not to cross and an attractive potential field associated with each set where the target is assumed to be confined in. For simplicity, assume the ownship is initially in the origin, considering some fixed frame of reference.
In detail, referring to
and
as the center of mass of
and
, respectively, the potential field at a point
is the superposition of a contribution
for each set
and a contribution
for each set
, i.e.,
from which the corresponding force at the origin is
and we note that the force is the composition of terms that are attracting toward the points
and terms that are repulsive from the points
.
Notice that, in this paper, we chose coefficients that are proportional to the area of the corresponding set (i.e., the larger is, the more the force is repulsive); conversely, we chose coefficients that are inversely proportional to the area of the corresponding set (i.e., the smaller is, the more the force is attractive).
Ownship Trajectory
Based on the resulting force
at the origin, the ownship computes the angle
between the vector
and
and moves according to
i.e., the ownship moves of linear motion (and constant velocity
) along the direction of
while performing sinusoidal motion around such a direction. Notably, Equation (
18) can be rearranged as
Figure 1 provides an example of the above procedure for the selection of
.
3. Experimental Analysis
In order to experimentally demonstrate the effectiveness of the proposed approach, we consider the scenario depicted in
Figure 2, where the target has an initial position
and moves with constant velocity having components
and
, while the acceleration is
.
Notice that we assume the additional information is available to the ownship regarding the target’s trajectory. Specifically, we assume the target’s trajectory lies in the intersection of the circles
and
and outside of the circle
; the centers
and the radii
and
of such circles are reported in
Table 1.
In our simulations, we consider a sampling time , a time horizon of and noise variance set to .
As for the ownship, we consider four operational scenarios:
No information: the ownship does not rely on the additional information and selects the trajectory in Equation (
19) with
.
Unconstrained, APF direction: the ownship does not rely on the additional information for the computations but selects the trajectory according to the proposed APF approach; in other words, it selects the trajectory in Equation (
19) with
.
Constrained, no APF direction: the ownship relies on the additional information for the computations but selects the trajectory in Equation (
19) with
.
Proposed Approach: the ownship follows the trajectory in Equation (
19) with
and, further to that, actively relies on the additional information during the computation of the MLE solution, of the experimental covariance matrix and CRLB.
In all cases, we select the following parameters for the ownship: , , and .
The computation of the MLE solution with MIDACO-SOLVER was conducted on an Intel® i7 quad-core @ 2.27 GHz. For each execution of MIDACO-SOLVER, we set the number of evaluated solutions to . All other MIDACO-SOLVER parameters were used by their default values, which especially means that a feasibility accuracy of was used for all individual constraints. In all cases, we compute the MLE solution with MIDACO-SOLVER, and we consider 100 MLE solutions for each operational scenario.
Table 2 reports the results obtained for the four operational scenarios, considering both the best solution found (in terms of the objective function of the MLE) over the 100 runs and the average of the solutions found. For each solution reported in the table, we use, as a measure of quality of the estimate the relative position and relative velocity, defined as follows:
while, since the target moves of with zero acceleration, we consider the absolute acceleration error
where the asterisk superscript is used for the estimated parameters. According to the table, the best solution found in the first and proposed operative scenarios is comparable and exhibits small error, while the error is larger for the third operational scenario. Conversely, within the second operational scenario, the best solution also found is not satisfactory, due to large relative velocity error. The situation is radically different considering the average of the found solutions; in fact, we observe that while the proposed approach shows limited error, the first and second operational conditions are characterized by erroneous average solutions (especially for what concerns the estimate of the target’s initial velocity), while the third one exhibits a limited but significant degradation. This suggests that, while the proposed approach consistently finds a good solution over the different trials, the other methods may become trapped by worst local minima.
This intuition is supported by
Figure 3, where we show the distribution of the position and velocity error values over the 100 trials. According to the figure, it can be noted that the first two scenarios have, overall, worse performance than the second and third scenario. Moreover, while outliers in the first two operative scenarios assume remarkably large values, in the latter two scenarios the outliers exhibit only a limited increase.
In order to further analyze the different operational scenarios,
Table 3 reports the norm of the covariance matrix obtained experimentally from the 100 trials, the experimental covariance limited to the solutions with errors within the 50th percentile, and the norm of the CRLB. In other words, we experimentally evaluate the covariance by executing 100 instances of MIDACO-SOLVER with random initial choice for the parameters and then taking the covariance of the 100 (or less, when only the 50th percentile is considered), possibly different, solutions obtained via MIDACO-SOLVER.
Notably, for the proposed approach, due to the presence of inequality constraints, the CRLB is in general set-valued and, following a worst-case philosophy, when the set is not a singleton, we consider
According to the table, the magnitude of the norm of the experimental covariance matrices associated with the proposed approach is three orders of magnitude smaller than those corresponding to the first and second operational scenarios, while it is two orders of magnitude smaller than the covariance associated to the third operational scenario. Moreover, the norm of the CRLB covariance matrices is two orders of magnitude smaller than the one obtained for the first and second operational scenarios, while it is comparable to the one associated with the third operational scenario. Moreover, we observe that, while in the other cases the experimental covariance is between three and four orders of magnitude larger than the CRLB, for the proposed approach, the experimental covariance is just two orders of magnitude larger than the CRLB. Notably, the discrepancy experienced between the experimental covariance computed over 100 trials and the CRLB one is due to the structure of the problem at hand. In fact, the MLE problem being solved amounts to a nonconvex, nonlinear programming problem and is thus characterized by the presence of local minima. Since we are adopting an approximated solver for finding a solution, the large experimental covariance is explained by the reach of different local minima across the different trials. In fact, while analyzing the covariance restricted to solutions with errors within the 50th percentile, we observe a significant drop with respect to the covariance over all trials; in particular, we observe a reduction of two orders of magnitude for the first two operational scenarios and one order of magnitude for the other two; moreover, also in this case, the proposed approach shows a two orders of magnitude reduction of the covariance with respect to the others.
Let us now discuss the computation times, which are collected in
Table 4. According to the table, we observe that the main differences arise between the unconstrained and the constrained cases, the latter requiring a computational time that is, on average, about
larger than the unconstrained case, while the standard deviation is sensibly larger, being about
times the one obtained in the unconstrained case.
Overall, the above results suggest that, while the implementation of the constrained MLE computation without considering the APF direction has a positive effect on the estimate, the APF direction alone has no particular benefit without constraining the MLE based on the available information. Instead, when such innovations are combined, the MLE error, the experimental covariance and the CRLB are greatly reduced.
Notably, since the proposed approach amounts to a constrained problem, the price to pay is a noticeable but limited increase in the computation times.
Figure 4,
Figure 5,
Figure 6,
Figure 7,
Figure 8 and
Figure 9 provide an at-a-glance view of the performance gap when the additional information is actively relied upon. Specifically,
Figure 4 and
Figure 5 show the results of the proposed operational scenario considering the average MLE solution over the 100 trials (blue dashed line), along with 100 trajectories (in cyan) obtained by sampling the parameters from a Gaussian distribution with mean given by the average MLE result and covariance corresponding to the experimental covariance matrix or the CRLB, respectively (the ownship’s trajectory is shown via the blue sinusoidal dashed curve). Conversely,
Figure 6 shows the results obtained considering the case where the APF direction is used but the additional information is not relied upon during the MLE computation; specifically, the figure considers the best solution found and the CRLB matrix. Notably, in the latter case, the large covariance yields sampled trajectories that are characterized by remarkably large error, while the proposed approach (both considering the experimental and CRLB covariance matrices) yields significantly better results.
Figure 7 shows a zoomed version of
Figure 6; according to the figure, we observe that, while the APF approach without using the additional information in the MLE computations is characterized by an initial position that is relatively accurate, the velocities and accelerations are characterized by highly variable and erroneous values, resulting in the inaccurate trajectories. Finally,
Figure 8 and
Figure 9 show the results obtained considering the case where the APF direction is not used but the additional information is relied upon during the MLE computation; specifically,
Figure 8 consider the best solution found and the experimental covariance matrix, while
Figure 8 considers the best solution found and the CRLB matrix; in this case, the trajectory is characterized by an intermediate error and is outperformed by the proposed approach.
To conclude the section, we compare the proposed approach against other methods in the literature. Specifically, our proposed comparison is based on two macro-steps: (i) the estimation of the target’s trajectory for via other approaches in the literature and (ii) the comparison with the trajectory obtained based on the estimation of the parameter vector via the proposed approach. In particular, we estimate the target’s trajectory by resorting to the following four algorithms:
Conversely, within the proposed approach, we estimate the parameter vector
via the proposed constrained MLE formulation, and we compute the trajectory of the target as follows
where
and
Regarding the initial condition for the algorithms compared against the proposed approach, we consider three different scenarios, with a decreasing degree of uncertainty:
- (1)
a scenario where the average initial condition
is drawn from a zero-mean Gaussian variable with standard deviation equal to
, while the initial covariance
is equal to the square of
, i.e.,
- (2)
a scenario where the average initial condition is drawn from a Gaussian variable with a mean equal to
and standard deviation equal to
, while the initial covariance is equal to the square of
, i.e.,
- (3)
a scenario where the average initial condition is exactly
, while the initial covariance is equal to the square of
, i.e.,
Figure 10,
Figure 11 and
Figure 12 report the results of the comparison, where the four aforementioned approaches are compared against the average MLE solution over 100 trials obtained via the proposed methodology, considering the same simulation setting as in the fourth operational scenario described above. Specifically, we report the relative position and velocity errors between the real target’s trajectory at time
k and the estimated one, i.e.,
where we denote by
and
, the estimate of the target’s positions and velocities obtained according to the generic technique being compared. According to
Figure 10, the proposed approach achieves an error that is at least two orders of magnitude less than the other approaches. Notably, while moving to a scenario where more information on the initial condition is available for the four approaches used in the comparison, the gap with PLKF and SL-EKF reduces to about one order of magnitude. Finally, when we compare the initial condition for the methods against the proposed one, which is assumed to have a mean that corresponds to the actual vector of parameter being estimated, we observe that the proposed approach is comparable with SL-EKF, while being in general better than the others.
Overall, the proposed comparison contributes to experimentally demonstrating the effectiveness of the proposed approach with respect to the literature.