Intelligence-Aware Batch Processing for TMA with Bearings-Only Measurements

Abstract

This paper develops a framework to track the trajectory of a target in 2D by considering a moving ownship able to measure bearing measurements. Notably, the framework allows one to incorporate additional information (e.g., obtained via intelligence) such as knowledge on the fact the target’s trajectory is contained in the intersection of some sets or the fact it lies outside the union of other sets. The approach is formally characterized by providing a constrained maximum likelihood estimation (MLE) formulation and by extending the definition of the Cramér–Rao lower bound (CRLB) matrix to the case of MLE problems with inequality constraints, relying on the concept of generalized Jacobian matrix. Moreover, based on the additional information, the ownship motion is chosen by mimicking the Artificial Potential Fields technique that 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). In order to show the effectiveness of the proposed approach, the paper is complemented by a simulation campaign where the MLE computations are carried out via an evolutionary ant colony optimization software, namely, mixed-integer distributed ant colony optimization solver (MIDACO-SOLVER). As a result, the proposed framework exhibits remarkably better performance, and in particular, we observe that the solution is less likely to remain stuck in unsatisfactory local minima during the MLE computation.

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:

$$\begin{array}{cc}\hfill x_t\left(k\right)& =x_{t0}+{\dot{x}}_{t0}kT+{\displaystyle \frac{1}{2}}{\ddot{x}}_t{k}^2{T}^2\hfill \\ \hfill y_t\left(k\right)& =y_{t0}+{\dot{y}}_{t0}kT+{\displaystyle \frac{1}{2}}{\ddot{y}}_t{k}^2{T}^2\hfill \\ \hfill {\dot{x}}_t\left(k\right)& ={\dot{x}}_{t0}+{\ddot{x}}_{t}kT\hfill \\ \hfill {\dot{y}}_t\left(k\right)& ={\dot{y}}_{t0}+{\ddot{y}}_{t}kT,\hfill \end{array}$$

(1)

with T being the sampling time. Moreover, let us consider an ownship platform aiming to estimate the parameter vector

$$\mathbf{\psi }={\left[\begin{array}{cccccc}{x}_{t0}& y_{t0}& {\dot{x}}_{t0}& {\dot{y}}_{t0}& {\ddot{x}}_t& {\ddot{y}}_t\end{array}\right]}^T,$$

(2)

based on a batch of measurements, sampled at uniform discrete-time instants $t=kT$ during the ownship motion and evaluated over the time interval $[0,k_{max}T]$.

In more detail, we assume that the ownship attempts to sense the following nominal measurement (e.g., see [36]):

$$h(\mathbf{\psi },k)=\mathrm{atan}2\left(y_t\left(k\right)-y_o\left(k\right),x_t\left(k\right)-x_o\left(k\right)\right)$$

(3)

where $x_o\left(k\right),y_o\left(k\right)$ are the coordinates of the ownship at time $t=kT$ 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:

$$z\left(k\right)=h(\mathbf{\psi },k)+w\left(k\right),$$

where the terms $w\left(k\right)\sim \mathcal{N}\left(0,{\sigma }^2\right)$ are independent and identically distributed Gaussian noises with zero-mean and variance ${\sigma }^2$.

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 $[0,k_{max}T]$, the position $\mathit{p}\left(k\right)={[x_t\left(k\right),y_t\left(k\right)]}^T$ of the target is confined in a region $\mathcal{P}$ of the plane defined as follows:

$$\mathcal{P}=\left\{\mathit{p}\in {\mathbb{R}}^2|\mathit{p}\in \bigcap _{i=1}^m{\mathcal{X}}_i\right\},$$

(4)

where the sets ${\mathcal{X}}_i\subseteq {\mathbb{R}}^2$ are convex, and their intersection is nonempty; moreover, the ownship is aware that the position $\mathit{p}\left(k\right)$ lies outside a region $\mathcal{S}$ of the plane defined as follows:

$$\mathcal{S}=\left\{\mathit{p}\in {\mathbb{R}}^2|\mathit{p}\in \bigcup _{i=1}^r{\mathcal{Y}}_i\right\},$$

(5)

where the sets ${\mathcal{Y}}_i\subseteq {\mathbb{R}}^2$ are convex.

The aim of this paper is to investigate how this additional information influences the estimation of $\mathbf{\theta }$ 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 $\mathbf{\psi }$ via the maximum likelihood estimate (MLE) technique (see, for instance [37], p. 182). The MLE for a vector parameter $\mathbf{\psi }$ is defined to be the value ${\mathbf{\theta }}^{*}$ that maximizes the likelihood function $p(z_1,z_2,\dots ,z_m,\mathbf{\theta })$ over the allowable domain of $\mathbf{\theta }$. In what follows, where understood, we abbreviate the notation by writing $p\left(\mathbf{\theta }\right)$. When $p\left(\mathbf{\theta }\right)$ is differentiable, we have that the MLE ${\mathbf{\theta }}^{*}$ satisfies

$${\displaystyle \frac{\partial ln\left(p\right(\mathbf{\theta }\left)\right)}{\partial \mathbf{\theta }}}{|}_{\mathbf{\theta }={\mathbf{\theta }}^{*}}={\mathbf{0}}_n.$$

(6)

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]:

$$\begin{array}{cc}\hfill p\left(\mathbf{\theta }\right)& ={\displaystyle \frac{1}{2\pi \sigma }}\prod _{k=1}^{k_{max}}exp\left(-{\displaystyle \frac{{(z\left(k\right)-h(\mathbf{\theta },k))}^2}{2{\sigma }^2}}\right).\hfill \end{array}$$

(7)

Notably, the maximization problem at hand can be formulated as

$${\mathbf{\theta }}^{*}=arg\underset{\mathbf{\theta }}{max}p\left(\mathbf{\theta }\right)=arg\underset{\mathbf{\theta }}{max}ln\left(p\left(\mathbf{\theta }\right)\right).$$

(8)

Let us define $\lambda \left(\mathbf{\theta }\right)=-ln\left(p\right(\mathbf{\theta }\left)\right)$. The above problem can be equivalently expressed as

$${\mathbf{\theta }}^{*}=arg\underset{\mathbf{\theta }}{min}\lambda \left(\mathbf{\theta }\right)$$

(9)

which, by simple computations is equivalent to solving [3]

$$\begin{array}{cc}\hfill {\mathbf{\theta }}^{*}& =arg\underset{\mathbf{\theta }}{min}\sum _{k=1}^{k_{max}}{\displaystyle \frac{{(z\left(k\right)-h(\mathbf{\theta },k))}^2}{2{\sigma }^2}}.\hfill \end{array}$$

(10)

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

$$\mathit{p}\left(k\right)=Q\left(k\right)\mathbf{\psi },$$

(11)

with

$$Q\left(k\right)=\left[\begin{array}{ccc}{I}_2& kT{I}_2& {\displaystyle \frac{1}{2}}{k}^2{T}^2{I}_2\end{array}\right]\in {\mathbb{R}}^{2\times 6}.$$

(12)

Therefore, by plugging the above expression in Equation (4), we have that $\mathit{p}\left(k\right)\in \mathcal{P}$ if and only if

$$Q\left(k\right)\mathbf{\psi }\in \mathcal{P},\forall k\in \{0,\dots ,k_{max}\},$$

(13)

while $\mathit{p}\left(k\right)\notin \mathcal{S}$ if and only if

$$Q\left(k\right)\mathbf{\psi }\notin \mathcal{S},\forall k\in \{0,\dots ,k_{max}\}.$$

In the following section, we assume that the constraints in the form

$$Q\left(k\right)\mathbf{\theta }\in \mathcal{P},Q\left(k\right)\mathbf{\theta }\notin \mathcal{S}$$

can be equivalently expressed as

$${\mathit{g}}_k\left(\mathbf{\theta }\right)=\mathit{g}\left(Q\left(k\right)\mathbf{\theta }\right)\le {\mathbf{0}}_q$$

for some $q\ge 0$ and for some differentiable function $\mathit{g}:{\mathbb{R}}^2\to {\mathbb{R}}^q$. In this view, the above unconstrained MLE problem can be extended as follows:

$$\begin{array}{ccc}\hfill & {\mathbf{\theta }}^{*}=arg\underset{\mathbf{\theta }}{min}\lambda \left(\mathbf{\theta }\right)\hfill & \\ \hfill & \mathrm{subject}\mathrm{to}\hfill & \\ \hfill & {\mathit{g}}_k\left(\mathbf{\theta }\right)\le {\mathbf{0}}_q,\forall k\in \{0,\dots ,k_{max}\}.\hfill \end{array}$$

(14)

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 $\mathbf{\psi }$ 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

$$E\left\{({\mathbf{\theta }}^{*}-\mathbf{\psi }){({\mathbf{\theta }}^{*}-\mathbf{\psi })}^T\right\}\ge J^{-1}\left(\overline{\mathbf{\psi }}\right)={\mathrm{CRLB}}^{\mathtt{unconstrained}},$$

where

$$J\left(\overline{\mathbf{\psi }}\right)=E\{\left[{\nabla }_{\mathbf{\theta }}\lambda \left(\mathbf{\theta }\right)\right]{\left[{\nabla }_{\mathbf{\theta }}\lambda \left(\mathbf{\theta }\right)\right]}^T\}{|}_{\mathbf{\theta }=\overline{\mathbf{\psi }}}.$$

Note that the expectations in the above equation are taken with respect to $p\left(\mathbf{\theta }\right)$; moreover, the derivatives in $J(·)$ are evaluated at the true value of $\mathbf{\psi }$ (i.e., $\overline{\mathbf{\psi }}=\mathbf{\psi }$) or, alternatively, at the estimated value ${\mathbf{\theta }}^{*}$ (i.e., $\overline{\mathbf{\psi }}={\mathbf{\theta }}^{*}$) 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 $\lambda \left(\mathbf{\theta }\right)$ with respect to $\mathbf{\theta }$ as [3]

$$\begin{array}{cc}\hfill {\nabla }_{\mathbf{\theta }}\lambda \left(\mathbf{\theta }\right)& =-\sum _{k=1}^{k_{max}}\frac{1}{{\sigma }^2}(z\left(k\right)-h(\mathbf{\theta },k)){\nabla }_{\mathbf{\theta }}h(\mathbf{\theta },k);\hfill \end{array}$$

(15)

then, we have that [3]

$$\begin{array}{cc}\hfill E\left\{\left[{\nabla }_{\mathbf{\theta }}\lambda \left(\mathbf{\theta }\right)\right]{\left[{\nabla }_{\mathbf{\theta }}\lambda \left(\mathbf{\theta }\right)\right]}^T\right\}{|}_{\mathbf{\psi }}& =\frac{1}{{\sigma }^4}\sum _{k=1}^{k_{max}}E\left\{{(z\left(k\right)-h(\mathbf{\psi },k))}^2\right\}{\nabla }_{\mathit{x}}h(\mathbf{\psi },k){\nabla }_{\mathbf{\psi }}h{(\mathbf{\psi },k)}^T\hfill \\ \hfill & =\frac{1}{{\sigma }^2}\sum _{k=1}^{k_{max}}{\nabla }_{\mathbf{\psi }}h(\mathbf{\psi },k){\nabla }_{\mathbf{\psi }}h{(\mathbf{\psi },k)}^T,\hfill \end{array}$$

(16)

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

$$E\left\{{(z\left(k\right)-h(\mathbf{\psi },k))}^2\right\}=E\left\{w{\left(k\right)}^2\right\}={\sigma }^2.$$

Notice that, in the Appendix A, we provide the analytical expression of the entries of ${\nabla }_{\mathbf{\psi }}h(\mathbf{\psi },k)$.

2.6. CRLB for Constrained MLE

As demonstrated in [33] (see also [34]), assuming $\mathbf{\theta }\in {\mathbb{R}}^n$, when the MLE problem has an equality constraint in the form

$$\mathit{f}\left(\mathbf{\theta }\right)={\mathbf{0}}_q,\mathit{f}:{\mathbb{R}}^n\to {\mathbb{R}}^q,$$

the CRLB can be computed starting from the unconstrained case, according to the following equation

$${\mathrm{CRLB}}^{\mathtt{constrained}}=J^{-1}\left(\overline{\mathbf{\psi }}\right)-J^{-1}\left(\overline{\mathbf{\psi }}\right)F\left(\overline{\mathbf{\psi }}\right){\left(F^T\left(\overline{\mathbf{\psi }}\right)J^{-1}\left(\overline{\mathbf{\psi }}\right)F\left(\overline{\mathbf{\psi }}\right)\right)}^{-1}{F}^T\left(\overline{\mathbf{\psi }}\right)J^{-1}\left(\overline{\mathbf{\psi }}\right),$$

where $F\left(\overline{\mathbf{\psi }}\right)$ is the $n\times q$ Jacobian matrix of the constraint function $\mathit{f}$, evaluated at $\overline{\mathbf{\psi }}$, i.e.,

$$F\left(\overline{\mathbf{\psi }}\right)={\nabla }_{\mathbf{\theta }}{\mathit{f}}^T\left(\mathbf{\theta }\right){|}_{\mathbf{\theta }=\overline{\mathbf{\psi }}}.$$

Let us now extend this method to the case of inequality constraints. To this end, consider a constraint in the form of

$$\mathit{g}\left(\mathbf{\theta }\right)\le {\mathbf{0}}_q$$

where $\mathit{g}:{\mathbb{R}}^n\to {\mathbb{R}}^q$ is differentiable. We point out that the inequality constraint can equivalently be expressed in the form of an equality constraint using

$$\mathit{f}\left(\mathbf{\theta }\right)=max\left\{\mathit{g}\left(\mathbf{\theta }\right),{\mathbf{0}}_q\right\}={\mathbf{0}}_q,$$

where max is intended component-wise. Notably, the max function is globally Lipschitz (e.g., see [43]); hence, if $\mathit{g}$ is differentiable everywhere, we have that $\mathit{f}$ is differentiable for all $\mathbf{\theta }\in {\mathbb{R}}^n\backslash \Omega$, where $\Omega$ is a zero-measure set in the form

$$\Omega =\left\{\mathbf{\theta }\in {\mathbb{R}}^n|g_i\left(\mathbf{\theta }\right)=0,\mathrm{for}\mathrm{some}i\in \{1,\dots ,q\}\right\}.$$

In this view, a natural extension is to resort to the generalized Jacobian of $\mathit{f}$. Specifically, it follows from [35] that the generalized Jacobian $F_G\left(\mathbf{\theta }\right)$ of $\mathit{f}\left(\mathbf{\theta }\right)$ is defined as

$$F_G\left(\mathbf{\theta }\right)=\overline{\mathtt{co}}\left\{\underset{i\to \infty }{lim}F\left({\mathbf{\theta }}_i\right):{\mathbf{\theta }}_i\to \mathbf{\theta },{\mathbf{\theta }}_i\notin \Omega \right\},$$

(17)

with $\overline{\mathtt{co}}$ being the convex closure, $F\left({\mathbf{\theta }}_i\right)\in {\mathbb{R}}^{n\times m}$ the classical Jacobian whenever it exists, and $\Omega$ the set of measure zero where $F\left({\mathbf{\theta }}_i\right)$ is not defined. In other words, $F_G\left(\mathbf{\theta }\right)$ is in general set-valued at the points where the Jacobian is not defined and $F_G\left(\mathbf{\theta }\right)=\left\{F\left(\mathbf{\theta }\right)\right\}$ when the Jacobian is defined. Consequently, in the case of inequality constraints, the CRLB is also, in general, set-valued, and it holds that

$${\mathrm{CRLB}}^{\mathtt{constrained}}=\left\{J^{-1}\left(\overline{\mathbf{\psi }}\right)-J^{-1}\left(\overline{\mathbf{\psi }}\right)F{\left(F^T{J}^{-1}\left(\overline{\mathbf{\psi }}\right)F\right)}^{-1}{F}^T{J}^{-1}\left(\overline{\mathbf{\psi }}\right)|F\in F_G\left(\overline{\mathbf{\psi }}\right)\right\}.$$

Notice that, in the event that $\overline{\mathbf{\psi }}$ coincides with a point in the zero-measure set $\Omega$, 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 ${\mathrm{CRLB}}^{\mathtt{constrained}}$.

2.7. Ownship Trajectory Selection Based on Artificial Potential Fields

The availability of additional information (i.e., knowledge on $\mathcal{P}$ and $\mathcal{S}$) 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 $\mathcal{P}$ and the will to avoid $\mathcal{S}$. 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 ${\mathcal{Y}}_i$ that the target is assumed not to cross and an attractive potential field associated with each set ${\mathcal{X}}_i$ 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 ${\mathit{y}}_i$ and ${\mathit{x}}_i$ as the center of mass of ${\mathcal{Y}}_i$ and ${\mathcal{X}}_i$, respectively, the potential field at a point $\mathit{p}\in {\mathbb{R}}^2$ is the superposition of a contribution

$$-{\displaystyle \frac{1}{2}}{\alpha }_i{({\mathit{x}}_i-\mathit{p})}^T({\mathit{x}}_i-\mathit{p})$$

for each set ${\mathcal{X}}_i$ and a contribution

$${\displaystyle \frac{1}{2}}{\beta }_i{({\mathit{y}}_i-\mathit{p})}^T({\mathit{y}}_i-\mathit{p})$$

for each set ${\mathcal{Y}}_i$, i.e.,

$$U\left(\mathit{p}\right)=-{\displaystyle \frac{1}{2}}\sum _{i=1}^m{\alpha }_i{({\mathit{x}}_i-\mathit{p})}^T({\mathit{x}}_i-\mathit{p})+{\displaystyle \frac{1}{2}}\sum _{i=1}^r{\beta }_i{({\mathit{y}}_i-\mathit{p})}^T({\mathit{y}}_i-\mathit{p}),$$

from which the corresponding force at the origin is

$$\mathit{f}\left({\mathbf{0}}_2\right)=-{\nabla }_{\mathit{p}}U\left(\mathit{p}\right){|}_{\mathit{p}={\mathbf{0}}_2}=\sum _{i=1}^m{\alpha }_i{\mathit{x}}_i-\sum _{i=1}^m{\beta }_i{\mathit{y}}_i,$$

and we note that the force is the composition of terms that are attracting toward the points ${\mathit{x}}_i$ and terms that are repulsive from the points ${\mathit{y}}_i$.

Notice that, in this paper, we chose coefficients ${\beta }_i$ that are proportional to the area of the corresponding set ${\mathcal{Y}}_i$ (i.e., the larger ${\mathcal{Y}}_i$ is, the more the force is repulsive); conversely, we chose coefficients ${\alpha }_i$ that are inversely proportional to the area of the corresponding set ${\mathcal{Y}}_i$ (i.e., the smaller ${\mathcal{X}}_i$ is, the more the force is attractive).

Ownship Trajectory

Based on the resulting force $\mathit{f}\left({\mathbf{0}}_2\right)$ at the origin, the ownship computes the angle $\gamma$ between the vector ${\left[\begin{array}{cc}0& 1\end{array}\right]}^T$ and $f\left({\mathbf{0}}_2\right)$ and moves according to

$$\left[\begin{array}{c}{x}_o\left(kT\right)\\ y_o\left(kT\right)\end{array}\right]=\left[\begin{array}{cc}cos\left(\gamma \right)& -sin\left(\gamma \right)\\ sin\left(\gamma \right)& cos\left(\gamma \right)\end{array}\right]\left[\begin{array}{c}{\overline{\dot{x}}}_{o}kT\\ Asin\left(\omega x_o\left(kT\right)\right)\end{array}\right],$$

(18)

i.e., the ownship moves of linear motion (and constant velocity ${\overline{\dot{x}}}_o$) along the direction of $\mathit{f}\left({\mathbf{0}}_2\right)$ while performing sinusoidal motion around such a direction. Notably, Equation (18) can be rearranged as

$$\left[\begin{array}{c}{x}_o\left(kT\right)\\ y_o\left(kT\right)\end{array}\right]=\left[\begin{array}{c}cos\left(\gamma \right){\overline{\dot{x}}}_{o}kT-Asin\left(\gamma \right)sin\left(\omega x_o\left(kT\right)\right)\\ sin\left(\gamma \right){\overline{\dot{x}}}_{o}kT+Acos\left(\gamma \right)sin\left(\omega x_o\left(kT\right)\right)\end{array}\right].$$

(19)

Figure 1 provides an example of the above procedure for the selection of $\mathit{f}\left({\mathbf{0}}_2\right)$.

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 ${\mathit{x}}_{t0}={\mathit{y}}_{t0}=3\times {10}^{4}m$ and moves with constant velocity having components ${\dot{\mathit{x}}}_{t0}=8.333m/s$ and ${\dot{\mathit{y}}}_{t0}=7.778m/s$, while the acceleration is ${\ddot{\mathit{x}}}_{t0}={\ddot{\mathit{y}}}_{t0}=0m/s^2$.

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 ${\mathcal{X}}_1$ and ${\mathcal{X}}_2$ and outside of the circle ${\mathcal{Y}}_1$; the centers ${\mathit{x}}_1,{\mathit{x}}_2,{\mathit{y}}_1$ and the radii ${\rho }_{{\mathcal{X}}_1},{\rho }_{{\mathcal{X}}_2}$ and ${\rho }_{{\mathcal{Y}}_1}$ of such circles are reported in

Table 1. Parameters describing the sets ${\mathcal{X}}_1$,${\mathcal{X}}_2$ and ${\mathcal{Y}}_1$ considered in the experimental analysis.

Set	Centroid	Radius
${\mathcal{X}}_1$	${\mathit{x}}_1={[{10}^4,5\times {10}^4]}^T\mathrm{m}$	${\rho }_{{\mathcal{X}}_1}=5\times {10}^4\mathrm{m}$
${\mathcal{X}}_2$	${\mathit{x}}_2={[2\times {10}^4,6\times {10}^4]}^T\mathrm{m}$	${\rho }_{{\mathcal{X}}_2}=4.5\times {10}^4\mathrm{m}$
${\mathcal{Y}}_1$	${\mathit{y}}_1={[4\times {10}^4,8\times {10}^4]}^T\mathrm{m}$	${\rho }_{{\mathcal{Y}}_1}=2.5\times {10}^4\mathrm{m}$

.

In our simulations, we consider a sampling time $T=2\begin{array}{c}\hfill \mathrm{s}\end{array}$, a time horizon of $3600\begin{array}{c}\hfill \mathrm{s}\end{array}$ and noise variance set to $\sigma =0.5^{\circ }$.

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 $\gamma =0\begin{array}{c}\hfill \mathrm{rad}\end{array}$.
• 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 $\gamma =0.749\begin{array}{c}\hfill \mathrm{rad}\end{array}$.
• Constrained, no APF direction: the ownship relies on the additional information for the computations but selects the trajectory in Equation (19) with $\gamma =0\begin{array}{c}\hfill \mathrm{rad}\end{array}$.
• Proposed Approach: the ownship follows the trajectory in Equation (19) with $\gamma =0.749\begin{array}{c}\hfill \mathrm{rad}\end{array}$ 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: $x_o\left(0\right)=y_o\left(0\right)=0\begin{array}{c}\hfill \mathrm{m}\end{array}$, ${\dot{x}}_o=7.778\begin{array}{c}\hfill \mathrm{m}/\mathrm{s}\end{array}$, $\omega =2.5\times {10}^{-4}\begin{array}{c}\hfill \mathrm{r}\mathrm{a}\mathrm{d}/\mathrm{s}\end{array}$ and $A=5\times {10}^3\begin{array}{c}\hfill \mathrm{m}\end{array}$.

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 ${10}^6$. All other MIDACO-SOLVER parameters were used by their default values, which especially means that a feasibility accuracy of $0.001$ 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. MLE parameter estimation via MIDACO-SOLVER for the different operational scenarios.

	${\mathit{x}}_{\mathit{t}0}\mathbf{m}$	${\mathit{y}}_{\mathit{t}0}\mathbf{m}$	${\dot{\mathit{x}}}_{\mathit{t}0}\mathbf{m}/\mathbf{s}$	${\dot{\mathit{y}}}_{\mathit{t}0}\mathbf{m}/\mathbf{s}$	${\ddot{\mathit{x}}}_{\mathit{t}}\mathbf{m}/\mathbf{s}$	${\ddot{\mathit{y}}}_{\mathit{t}}\mathbf{m}/\mathbf{s}$	rel. pos. err.	rel. vel. err.	abs. acc. err.
Ground truth	$3.000\times {10}^4$	$3.000\times {10}^4$	$8.333$	$7.778$	$0.000$	$0.000$	-	-	-
No information, average	$3.933\times {10}^4$	$3.848\times {10}^4$	$-5.048$	$-4.123$	$3.437\times {10}^{-3}$	$2.303\times {10}^{-3}$	$4.202\times {10}^{-1}$	$2.218$	$4.137\times {10}^{-3}$
Unconstrained, APF direction, average	$3.796\times {10}^4$	$3.733\times {10}^3$	$-4.714$	$-4.143$	$4.780\times {10}^{-3}$	$3.958\times {10}^{-3}$	$3.605\times {10}^{-1}$	$2.191$	$6.206\times {10}^{-3}$
Constrained, no APF direction, average	$3.204\times {10}^4$	$3.193\times {10}^4$	$7.161$	$6.884$	$2.543\times {10}^{-4}$	$4.987\times {10}^{-5}$	$9.384\times {10}^{-2}$	$1.815\times {10}^{-1}$	$2.591\times {10}^{-4}$
Proposed Approach, average	$3.040\times {10}^4$	$3.035\times {10}^4$	$7.912$	$7.382$	$2.652\times {10}^{-4}$	$2.598\times {10}^{-4}$	$1.767\times {10}^{-2}$	$7.169\times {10}^{-2}$	$3.713\times {10}^{-4}$
No information, best	$3.028\times {10}^4$	$3.024\times {10}^4$	$8.045$	$7.507$	$1.989\times {10}^{-4}$	$1.969\times {10}^{-4}$	$1.243\times {10}^{-2}$	$4.902\times {10}^{-2}$	$2.789\times {10}^{-4}$
Unconstrained, APF direction, best	$3.704\times {10}^4$	$3.640\times {10}^4$	$0.686$	$0.618$	$3.754\times {10}^{-3}$	$3.560\times {10}^{-3}$	$3.171\times {10}^{-1}$	$1.299$	$5.173\times {10}^{-3}$
Constrained, no APF direction, best	$2.811\times {10}^4$	$2.806\times {10}^4$	$10.081$	$7.382$	$-1.460\times {10}^{-3}$	$-2.202\times {10}^{-3}$	$9.023\times {10}^{-2}$	$3.412\times {10}^{-1}$	$2.642\times {10}^{-3}$
Proposed Approach, best	$3.026\times {10}^4$	$3.023\times {10}^4$	$8.070$	$7.531$	$1.835\times {10}^{-4}$	$1.821\times {10}^{-4}$	$1.179\times {10}^{-2}$	$4.471\times {10}^{-2}$	$2.586\times {10}^{-4}$

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:

$$\mathrm{rel}.\mathrm{pos}.\mathrm{err}.={∥\left[\begin{array}{c}{\displaystyle \frac{x_{t0}-x_{t0}^{*}}{x_{t0}}}\\ {\displaystyle \frac{y_{t0}-y_{t0}^{*}}{y_{t0}}}\end{array}\right]∥}_2$$

(20)

$$\mathrm{rel}.\mathrm{vel}.\mathrm{err}.={∥\left[\begin{array}{c}{\displaystyle \frac{{\dot{x}}_{t0}-{\dot{x}}_{t0}^{*}}{{\dot{x}}_{t0}}}\\ {\displaystyle \frac{{\dot{y}}_{t0}-{\dot{y}}_{t0}^{*}}{{\dot{y}}_{t0}}}\end{array}\right]∥}_2$$

(21)

while, since the target moves of with zero acceleration, we consider the absolute acceleration error

$$\mathrm{abs}.\mathrm{acc}.\mathrm{err}.={∥\left[\begin{array}{c}{\ddot{x}}_t^{*}\\ {\ddot{y}}_t^{*}\end{array}\right]∥}_2$$

(22)

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. Euclidean norm of the parameter estimation via MIDACO-SOLVER for the different operational scenarios.

	Experimental Covariance ${\parallel ·\parallel }_2$	Experimental Covariance ${\parallel ·\parallel }_2$	CRLB ${\parallel ·\parallel }_2$
		(Solutions with Errors ≤ 50th Percentile)
No information	$3.851\times {10}^8$	$3.743\times {10}^6$	$7.713\times {10}^5$
Unconstrained, APF direction	$5.666\times {10}^8$	$4.144\times {10}^6$	$7.713\times {10}^5$
Constrained, no APF direction	$3.696\times {10}^7$	$5.410\times {10}^6$	$1.454\times {10}^3$
Proposed Approach	$4.784\times {10}^5$	$6.600\times {10}^4$	$1.873\times {10}^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

$$\underset{C\in {\mathrm{CRLB}}^{\mathtt{constrained}}}{max}{\parallel C\parallel }_2.$$

(23)

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. Average and standard deviation over 100 trials of the computational time (in seconds) for the computation of the MLE solution via MIDACO-SOLVER for the different operational scenarios.

	Time (Average) $\mathbf{s}$	Time (Standard Deviation) $\mathbf{s}$
No information	$58.790$	$3.235$
Unconstrained, APF direction	$59.124$	$3.049$
Constrained, no APF direction	$85.926$	$21.231$
Proposed Approach	$86.230$	$22.395$

. 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 $46\%$ larger than the unconstrained case, while the standard deviation is sensibly larger, being about $6.92$ 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 $k\in \{0,\dots k_{max}\}$ via other approaches in the literature and (ii) the comparison with the trajectory obtained based on the estimation of the parameter vector $\mathbf{\psi }$ via the proposed approach. In particular, we estimate the target’s trajectory by resorting to the following four algorithms:

• a standard extended Kalman filter (EKF) (e.g., see [44] and references therein), where we approximate the nonlinear output function $h(·)$ by its Jacobian matrix at each time instant;
• the pseudolinear Kalman filter (PL-KF) [45], where the nonlinear and noisy output $z\left(k\right)=\begin{array}{c}\hfill \mathrm{atan}2(y_t\left(k\right)-y_o\left(k\right),x_t\left(k\right)-x_o\left(k\right))\end{array}+w\left(k\right)$ is approximated by

  $$\tilde{z}\left(k\right)=\left[\begin{array}{c}sin\left(z\right(k\left)\right)\\ -cos\left(z\right(k\left)\right)\end{array}\right]\underset{M}{\underbrace{\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\end{array}\right]}}{\widehat{\mathit{x}}}_{k|k}+\eta \left(k\right),$$

  where ${\widehat{\mathit{x}}}_{k|k}$ is the vector collecting the filtered states (i.e., the stack of the positions, velocities and accelerations) of the target at time k and $\eta \left(k\right)$ is a pseudolinear noise in the form

  $$\eta \left(k\right)\sim \mathcal{N}\left(0,R_k\right),$$

  with

  $$R_k\approx {\parallel {\widehat{\mathit{d}}}_{k|k-1}\parallel }^2{\sigma }^2$$

  and

  $${\widehat{\mathit{d}}}_{k|k-1}=M{\widehat{\mathit{x}}}_{k|k-1}-\left[\begin{array}{c}{x}_o\left(k\right)\\ y_o\left(k\right)\end{array}\right],$$
  ${\widehat{\mathit{x}}}_{k|k-1}$ being the vector collecting the prediction of the target’s states at time k;
• a statistical linearization extended Kalman filter (SL-EKF) [46] where, instead of the Jacobian of the output function $h(·)$, we approximate the nonlinear measurement function $y\left(k\right)=h(\mathit{x},k)+w\left(k\right)$ via the linear approximation $H^{*}\mathit{x}$, with

  $$H^{*}=\underset{H\in {\mathbb{R}}^{2\times 6}}{argmin}{\parallel y\left(k\right)-H{\widehat{\mathit{x}}}_{k|k}\parallel }^2.$$
• the shifted Rayleigh filter (SRF) [47], where $z\left(k\right)$ is approximated by

  $$z\left(k\right)\approx \Pi \left(M\mathit{x}\left(k\right)+\mathit{u}\left(k\right)+\omega \left(k\right)\right),$$

  where $\Pi \left(\mathit{r}\right)=\mathit{r}/\parallel \mathit{r}\parallel$, $\mathit{u}\left(k\right)=\left[\begin{array}{cc}{x}_o\left(k\right)& y_o\left(k\right)\end{array}\right]$ and

  $$\omega \left(k\right)\sim \mathcal{N}\left({\mathbf{0}}_2,{\sigma }^{2}E\left[{\parallel M\mathit{x}\left(k\right)+\mathit{u}\left(k\right)\parallel }^2|z\left(1\right),\dots ,z\left(k\right)\right]I_2\right).$$

Conversely, within the proposed approach, we estimate the parameter vector

$$\mathbf{\psi }={\left[\begin{array}{cccccc}{x}_{t0}& y_{t0}& {\dot{x}}_{t0}& {\dot{y}}_{t0}& {\ddot{x}}_t& {\ddot{y}}_t\end{array}\right]}^T$$

via the proposed constrained MLE formulation, and we compute the trajectory of the target as follows

$$\mathit{x}\left(k\right)=Q\left(k\right)\mathbf{\psi },$$

where

$$\mathit{x}\left(k\right)={\left[\begin{array}{cccccc}{x}_t\left(k\right)& y_t\left(k\right)& {\dot{x}}_t\left(k\right)& {\dot{y}}_t\left(k\right)& {\ddot{x}}_t\left(k\right)& {\ddot{y}}_t\left(k\right)\end{array}\right]}^T,$$

and

$$Q\left(k\right)=\left[\begin{array}{ccc}{I}_2& kT{I}_2& {\displaystyle \frac{1}{2}}{k}^2{T}^2{I}_2\\ 0_{2\times 2}& I_2& kT{I}_2\\ 0_{2\times 2}& 0_{2\times 2}& I_2\end{array}\right].$$

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 ${\widehat{x}}_{0|0}$ is drawn from a zero-mean Gaussian variable with standard deviation equal to $\mathbf{\psi }$, while the initial covariance ${\Sigma }_{0|0}$ is equal to the square of $\mathbf{\psi }$, i.e.,

$${\widehat{x}}_{0|0}\sim \mathcal{N}\left({\mathbf{0}}_6,\begin{array}{c}\hfill \mathrm{diag}\end{array}{\left(\mathbf{\psi }\right)}^2\right),{\Sigma }_{0|0}=\begin{array}{c}\hfill \mathrm{diag}\end{array}{\left(\mathbf{\psi }\right)}^2;$$

(2)

a scenario where the average initial condition is drawn from a Gaussian variable with a mean equal to $\mathbf{\psi }$ and standard deviation equal to $\mathbf{\psi }$, while the initial covariance is equal to the square of $\mathbf{\psi }$, i.e.,

$${\widehat{x}}_{0|0}\sim \mathcal{N}\left(\mathbf{\psi },\begin{array}{c}\hfill \mathrm{diag}\end{array}{\left(\mathbf{\psi }\right)}^2\right),{\Sigma }_{0|0}=\begin{array}{c}\hfill \mathrm{diag}\end{array}{\left(\mathbf{\psi }\right)}^2$$

(3)

a scenario where the average initial condition is exactly $\mathbf{\psi }$, while the initial covariance is equal to the square of $\mathbf{\psi }$, i.e.,

$${\widehat{x}}_{0|0}=\mathbf{\psi },{\Sigma }_{0|0}=\begin{array}{c}\hfill \mathrm{diag}\end{array}{\left(\mathbf{\psi }\right)}^2.$$

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

$${\displaystyle \frac{x_t\left(k\right)-\widehat{x}\left(k\right)}{x_t\left(k\right)}},{\displaystyle \frac{y_t\left(k\right)-\widehat{y}\left(k\right)}{y_t\left(k\right)}},{\displaystyle \frac{{\dot{x}}_t\left(k\right)-\widehat{\dot{x}}\left(k\right)}{{\dot{x}}_t\left(k\right)}},{\displaystyle \frac{{\dot{y}}_t\left(k\right)-\widehat{\dot{y}}\left(k\right)}{{\dot{y}}_t\left(k\right)}},$$

where we denote by $\widehat{x}\left(k\right),\widehat{y}\left(k\right),\widehat{\dot{x}}\left(k\right)$ and $\widehat{\dot{y}}\left(k\right)$, 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.

4. Conclusions and Future Work

This paper presents a batch strategy to estimate the parameters describing the trajectory of a target, based on a moving ownship able to measure bearings. In particular, the proposed methodology allows one to incorporate additional information (e.g., obtained via intelligence) such as knowledge of the fact that the target’s trajectory is contained in the intersection of some sets or the fact it lies outside the union of other sets. The approach is formally characterized by providing a constrained MLE formulation and by extending the definition of the CRLB matrix to the case of MLE problems with inequality constraints, relying on the concept of generalized Jacobian matrix. Moreover, based on the additional information, the ownship motion is chosen by mimicking the Artificial Potential Fields technique that is typically used by mobile robots to aim to 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 with the target’s trajectory). As a result, the proposed framework exhibits remarkably better performance, and in particular, we observe that the solution is less likely to remain stuck in unsatisfactory local minima during the MLE computation and is characterized by smaller covariance, (both considering the experimental and the CRLB ones).

Future work will be mainly devoted to extending the framework along the following research directions: (i) consider more sophisticated models for the target’s motion (e.g., nearly constant acceleration); (ii) consider dynamically changing constraints on the target’s trajectory; (iii) provide adaptive strategies for the ownship trajectory based on the, although partial, initial estimates (e.g., in order to avoid crossing the space where the target’s trajectory is contained); (iv) filter possible outliers (e.g., resorting to the approach in [3], Section 2.7); and (v) consider multiple targets.