Ground Moving Target Tracking Filter Considering Terrain and Kinematics

Abstract

This paper addresses ground target tracking (GTT) for airborne radar. Digital terrain elevation data (DTED) are widely used for GTT as prior information under the premise that ground targets are constrained on terrain. Existing works fuse DTED to a tracking filter in a way that adopts only the assumption that the position of the target is constrained on the terrain. However, by kinematics, it is natural that the velocity of the moving ground target is constrained as well. Furthermore, DTED provides neither continuous nor accurate measurement of terrain elevation. To overcome such limitations, we propose a novel soft terrain constraint and a constraint-aided particle filter. To resolve the difficulties in applying the DTED to the GTT, first, we reconstruct the ground-truth terrain elevation using a Gaussian process and treat DTED as a noisy observation of it. Then, terrain constraint is formulated as joint soft constraints of position and velocity. Finally, we derive a Soft Terrain Constrained Particle Filter (STC-PF) that propagates particles while approximately satisfying the terrain constraint in the prediction step. In the numerical simulations, STC-PF outperforms the Smoothly Constrained Kalman Filter (SCKF) in terms of tracking performance because SCKF can only incorporate hard constraints.

1. Introduction

Ground tracking radars mounted on airborne platforms play a key role in many applications, especially those for military purposes; surveillance, airstrike, and escort missions done by aircraft commonly require precise tracking of ground targets. In several modern military campaigns, ground moving target indicator (GMTI) radar on-board the Joint Surveillance Target Attack Radar System (STARS) has been proven strategically and tactically significant [1]. Accordingly, algorithms that track ground targets running on radars are becoming more important. Although there have been great advances in target tracking, tracking ground targets is still a challenging problem. The reason is that the characteristics of ground target tracking are different from those of tracking other types of targets. (e.g., high clutter, terrain obscuration, etc.) [2].

Because exploiting appropriate assumptions other than the state-space model can help to improve the statistical inferences of the system [3], many studies have tried to introduce useful assumptions to ground target tracking. They can be classified based on two criteria of what or how assumptions were applied.

Based on the first criteria, existing studies can be classified into two further categories. The first category considers the behavior characteristics of the ground target that are distinguished from those of airborne targets. Fosbury [4] and Kastella [5] each created the terms ‘trafficability’ and ‘hospitability of maneuver’, which represent how easily a vehicle can go through a particular area. These notions adaptively modify the target dynamics so that the dynamics can reflect the tendency of the target to prefer directions with small gradients. The second category involves empirical constraints. For instance, in the work of Streller [6], ground targets are assumed to move along the infrastructure such as roads, bridges, etc. More specifically, the assumption encourages the prior probability density which is propagated by the system model to align with the road network. The same assumption is utilized in the works of Pannetier [7,8]. The works of Mallick [9] and Kim [10] are also classified into the same category and share the same motivation as ours. To compensate for inaccurate GMTI measurements, both utilized the assumption that the position of a ground target is restricted to the terrain surface. This idea can be extended even further by adding another assumption that the velocity of the target is tangent to the terrain surface, which allowing the system to estimate the velocity more precisely [11,12].

From an other perspective, existing studies can be classified based on the second criteria, namely, how assumptions are applied. The first category involves modifying the target dynamics so that it can reflect the tendency of the target. Similar to the aforementioned works [4,5,6,13], the system dynamics of the filter are adaptively modified. In other words, external knowledge is embedded in the state-space model. Thus, we have the freedom to control only the tendency of a target. The second category involves transforming the assumption into a state constraint. This type of approach explicitly limits the state of a target to a specific subspace ([7,8] for example).

Extensive studies have attempted to deal with such constrained state estimation problems [11], including the methods that do not rely on the state-space model [14,15]. In the case of linear system dynamics and linear constraints, the following methods are applicable: model reduction [16], perfect measurement [17,18,19], estimate [20]/system [21]/gain [22] projection, pdf truncation [22], etc. If either system dynamics or constraint is nonlinear, the combination of linearization and linear methods is an available option. Other possible choices are variants of the Unscented Kalman Filter (PUKF [12,23], ECUKF [12], 2UKF [24], etc.), variants of the Particle Filter [25,26,27,28] (CLIP, COMP [29]), and the Smoothly Constrained Kalman Filter (SCKF) [30]. Moreover, many works in the literature have paid attention to state estimation problems with soft constraints [18,19,31,32,33,34]. Soft constraints, conditions that the state approximately satisfies, are utilized in most practical engineering applications [11,33] because uncertainty may appear during the transformation of external knowledge into the constraint. For example, in the case of ground target tracking constrained to a road, the roadmap may be inaccurate. Among promising methods dealing with soft constraints, some regard the degree of constraint satisfaction as measurement and extend the likelihood function [18,19,31,32,35]. Especially, this approach can be intuitively extended to a nonlinear soft constraint; scPF (soft-constrained Particle Filter) [35] is a good example. scPF has the advantage of preserving the nonlinearity of the constraint because it is based on an SIR (Sequential Importance Resampling) particle filter. However, scPF is not sample-efficient because the constraint is reflected by the generalized likelihood. More specifically, while particles are propagated through the system dynamics, they can be scattered in a direction that does not satisfy the constraint. Therefore, the propagated particles that do not satisfy the constraint would be given a low likelihood and eventually vanish, which makes the whole algorithm inefficient.

Thus, in this paper, we propose a particle filter that considers the stochastic terrain constraint. The term ‘terrain constraint’ not only represents the assumption that the position of a ground target should be located on the terrain surface but also that the velocity vector of the target should be tangent to the terrain surface. Contributions are the following:

• We propose a sample-efficient particle filter to which the terrain constraint can be applied. The proposed algorithm is named Soft Terrain Constrained Particle Filter (STC-PF). Given the assumption of target motion, STC-PF performs sampling in a direction for which the state satisfies the constraint during the propagation step. As a result, STC-PF is more sample-efficient than scPF. Furthermore, in the numerical simulations, STC-PF using soft terrain constraint outperforms Smoothly Constrained Kalman Filter (SCKF) [30] using hard constraint in terms of tracking performance.
• Using a Gaussian process, terrain constraint is formulated as a soft position constraint along with a soft velocity constraint. Because kinematics states that position and velocity is not independent, a constraint on the position of a target implies that the velocity of the target will be constrained as well. Therefore, terrain constraint includes both position constraint and velocity constraint. Furthermore, terrain constraint requires exact terrain elevation and its gradient at an arbitrary position, but DTED (Digital Terrain Elevation Data) [36] cannot provide them. To overcome this issue, we model the ground-truth terrain elevation with a Gaussian process (GP) and treat DTED as a noisy observation [37] of it.

Technically, we used SRTM (Shuttle Radar Topography Mission). However, we will use the term DTED and SRTM interchangeably as they both are data that map terrain elevation of the entire globe.

The structure of this paper is as follows: In Section 2, tracking of a ground target with a terrain constraint is formulated. Section 3 presents the proposed algorithm, STC-PF. Section 4 provides detailed explanations, the results, and a discussion of the numerical simulation. Finally, in Section 5, we conclude.

2. Problem Formulation

In this section, tracking of a ground target with terrain constraint is formulated as a constrained state estimation problem.

Consider a system described by the following state-space model:

$${\mathrm{x}}_{k+1}=\mathrm{f}\left({\mathrm{x}}_k\right)+{\mathrm{w}}_k$$

(1)

$${\mathrm{y}}_k=\mathrm{g}\left({\mathrm{x}}_k\right)+{\mathrm{n}}_k$$

(2)

where ${\mathrm{x}}_k$ is the system state vector at time k, ${\mathrm{y}}_k$ the measurement vector, $\mathrm{f}$ the system function, $\mathrm{g}$ the observation function, ${\mathrm{w}}_k$ the process noise vector, and ${\mathrm{n}}_k$ the measurement noise vector. The system state vector ${\mathrm{x}}_k\in {\mathbf{R}}^6$ consists of the position ($x_k$, $y_k$, $z_k$) and the velocity ($v_{x,k}$, $v_{y,k}$, $v_{z,k}$) in local Cartesian coordinates at time k. The system function is a possibly nonlinear function but is assumed to be a constant velocity model in this paper. ${\mathrm{y}}_k\in {\mathbf{R}}^3$ is the measurement, which consists of range, azimuth angle, and elevation angle measured from the radar. ${\mathrm{w}}_k\sim N(0,Q)$ is white Gaussian process noise, and ${\mathrm{n}}_k\sim N(0,R)$ is white Gaussian measurement noise. Subsequently, Equations (1) and (2) are realized as follows:

$${\mathrm{x}}_{k+1}=\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& \Delta t·{\mathbf{I}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\right]{\mathrm{x}}_k+{\mathrm{w}}_k$$

(3)

$${\mathrm{y}}_k=\left[\begin{array}{c}\sqrt{x_k^2+y_k^2+z_k^2}\\ arctan\frac{y_k}{x_k}\\ arcsin\frac{z_k}{\sqrt{x_k^2+y_k^2+z_k^2}}\end{array}\right]+{\mathrm{n}}_k.$$

(4)

The final goal of the state estimation problem is to infer the state sequence of the dynamical system ${\mathrm{x}}_{0:k}$ from the series of observations ${\mathrm{y}}_{1:k}$.

Now, the terrain constraint can come into play to incorporate the additional information that the state-space model cannot reflect. The terrain constraint not only represents the assumption that the position of a ground target should be located on the terrain surface but also that the velocity vector of the target should be tangent to the terrain surface. Both assumptions can be transformed into state constraints as follows:

$$\begin{array}{c}{h}_k=\overline{h}({\lambda }_k,{\phi }_k)\hfill \\ v_{h,k}=\nabla \overline{h}({\lambda }_k,{\phi }_k)·{\left[\begin{array}{cc}{v}_{\lambda ,k}& v_{\phi ,k}\end{array}\right]}^T\hfill \end{array}$$

(5)

where ${\lambda }_k$, ${\phi }_k$, and $h_k$ are the latitude, longitude, and altitude (LLA) of the target at time k. $\overline{h}(\lambda ,\phi )$ is ground-truth terrain elevation at latitude $\lambda$ and longitude $\phi$. Note that we do not have direct access to $\overline{h}$, but only noisy observations,

$$D=\left\{\mathrm{DTED}({\lambda }^i,{\phi }^i)\right|i=1\cdots N_D\}$$

(6)

such that

$$\mathrm{DTED}(\lambda ,\phi )=\overline{h}(\lambda ,\phi )+ϵ(\lambda ,\phi ).$$

(7)

3. Soft Terrain Constrained Particle Filter

In this section, the newly proposed algorithm, Soft Terrain Constrained Particle Filter (STC-PF) is derived. In Section 3.1, mathematical modeling of ground-truth terrain elevation is presented. Then, we propose a technique for the transformation of velocity between the LLA coordinates and the local Cartesian coordinates in Section 3.2. Necessary assumptions required for algorithm derivation are described in Section 3.3. After the algorithm derivation in Section 3.4, we show the similarity between STC-PF and scPF [35] in Section 3.5.

3.1. Modeling of Ground-Truth Terrain Elevation

Although the terrain constraint (Equation (5)) requires the ground-truth elevation, it is almost impossible in practice to retrieve it at an arbitrary position. The reason is that DTED provides neither accurate ground-truth terrain elevation (Equation (7)) nor terrain elevation at arbitrary positions. (Equation (6)) This challenge can be met by reconstructing the ground-truth terrain elevation with a Gaussian process (GP) and treating the DTED as independent observations:

$$\begin{array}{c}\overline{h}\sim GP\left(m(\lambda ,\phi ),k((\lambda ,\phi ),({\lambda }^{\prime },{\phi }^{\prime }))\right)\hfill \\ \mathrm{DTED}(\lambda ,\phi )=\overline{h}(\lambda ,\phi )+ϵ\hfill \\ ϵ\sim N(0,{\sigma }_{\mathrm{DTED}})\hfill \end{array}$$

(8)

where the observation noise ${\sigma }_{\mathrm{DTED}}$ can be estimated from the work of Rodriguez [37]. (see Appendix B) Because GP assigns a probability for each possible terrain, the terrain constraint becomes stochastic. An advantage of this approach is that it enables us to compute the gradient of $\overline{h}$ analytically, which is required to apply the velocity constraint. (Equation (5)) More strictly, joint predictive distribution for ground-truth terrain elevation and its gradient can be expressed in a closed-form, (detailed description is in Appendix A)

$$\overline{h},\nabla \overline{h}|\mathrm{DTED}\sim N(\overline{\mu },\overline{\mathrm{\Sigma }})$$

(9)

provided that the kernel function is differentiable. Figure 1 shows an example of prediction results when zero mean function and squared exponential kernel are utilized.

$$\begin{array}{cc}m(\lambda ,\phi )& =\mathbb{E}\left[\overline{h}(\lambda ,\phi )\right]\hfill \\ & =0\hfill \\ k((\lambda ,\phi ),({\lambda }^{\prime },{\phi }^{\prime }))& =\mathbb{E}\left[\left(\overline{h}(\lambda ,\phi )-m(\lambda ,\phi )\right)\left(\overline{h}({\lambda }^{\prime },{\phi }^{\prime })-m({\lambda }^{\prime },{\phi }^{\prime })\right)\right]\hfill \\ & =\alpha exp\left(-\frac{1}{2}\left[\begin{array}{cc}\lambda -{\lambda }^{\prime }& \phi -{\phi }^{\prime }\end{array}\right]\mathrm{\Gamma }\left[\begin{array}{c}\lambda -{\lambda }^{\prime }\\ \phi -{\phi }^{\prime }\end{array}\right]\right)\hfill \end{array}$$

(10)

3.2. Velocity Transformation

Another major challenge when applying the terrain constraint to the filter is that the conversion of velocity between the local Cartesian coordinates and the LLA coordinates is not straightforward. More specifically, the terrain constraint (Equation (5)) requires the velocity in LLA coordinates.

This challenge can be met by multiplying the Jacobian, which is obtained by numerical differentiation. Additionally, because velocity in local Cartesian coordinates is relative while that in LLA coordinates is absolute, the velocity of the radar $V_{lla,ownship}$ should be added (or subtracted) after (or before) multiplying by the Jacobian.

$$\begin{array}{cc}{\left[\begin{array}{c}\frac{\partial \lambda }{\partial t}\\ \frac{\partial \phi }{\partial t}\\ \frac{\partial h}{\partial t}\end{array}\right]}_{rel}& =\left[\begin{array}{ccc}\frac{\partial \lambda }{\partial x}& \frac{\partial \lambda }{\partial y}& \frac{\partial \lambda }{\partial z}\\ \frac{\partial \phi }{\partial x}& \frac{\partial \phi }{\partial y}& \frac{\partial \phi }{\partial z}\\ \frac{\partial h}{\partial x}& \frac{\partial h}{\partial y}& \frac{\partial h}{\partial z}\end{array}\right]\left[\begin{array}{c}\frac{\partial x}{\partial t}\\ \frac{\partial y}{\partial t}\\ \frac{\partial z}{\partial t}\end{array}\right]\hfill \\ & =J_{xyz2lla}{V}_{xyz}\hfill \\ V_{lla}& =J_{xyz2lla}{V}_{xyz}+V_{lla,ownship}\hfill \end{array}$$

(11)

Conversion from LLA to local Cartesian can be done in a converse way.

$$V_{xyz}=J_{lla2xyz}\left(V_{lla}-V_{lla,ownship}\right)$$

(12)

where $J_{lla2xyz}=J_{xyz2lla}^{-1}$.

3.3. Assumptions

Regarding the motion of the target, we assume the followings:

• The vertical position (h) can be determined provided that the horizontal position ($\lambda$, $\phi$) is fixed.
• Then, the vertical velocity ($v_h$) can be also determined when the horizontal velocity ($v_{\lambda }$, $v_{\phi }$) is fixed.

In Figure 2, assumptions 1 and 2 correspond to the red arrows that inbound to h and $v_h$, respectively. They comprise the ’elevation model’.

Due to the recursive Markovian structure, it is possible to infer the current latent state from the previously inferred latent state and the current measurement. Mathematically, by Bayes’ rule, the joint distribution of ${\mathrm{x}}_{0:k}$ given ${\mathrm{y}}_{1:k}$ can be expressed as

$$\begin{array}{cc}P\left({\mathrm{x}}_{0:k}\right|{\mathrm{y}}_{1:k})\propto \hfill & \phantom{·}P\left({\mathrm{y}}_k\right|{\lambda }_k,{\phi }_k,h_k)\hfill \\ \hfill & ·P({\lambda }_k,{\phi }_k,v_{\lambda ,k},v_{\phi ,k}|{\mathrm{x}}_{k-1})\hfill \\ \hfill & ·P\left(h_k\right|{\lambda }_k,{\phi }_k)·P\left(v_{h,k}\right|{\lambda }_k,{\phi }_k,v_{\lambda ,k},v_{\phi ,k})\hfill \\ \hfill & ·P\left({\mathrm{x}}_{0:k-1}\right|{\mathrm{y}}_{0:k-1})\hfill \end{array}$$

(13)

The dynamic model $P({\lambda }_k,{\phi }_k,v_{\lambda ,k},v_{\phi ,k}|{\mathrm{x}}_{k-1})$ and the likelihood model $P\left({\mathrm{y}}_k\right|{\lambda }_k,{\phi }_k,h_k)$ are found in the above equation. Respectively, they correspond to the blue arrows and the green arrows in Figure 2. Note that the measurement ${\mathrm{y}}_k$ is only affected by the position of the target (${\lambda }_k$, ${\phi }_k$, $h_k$), as stated in Equation (4).

3.4. Algorithm

The proposed algorithm is based on the SIR(Sequential Importance Resampling) particle filter. In the SIR algorithm, which forms the basis of most sequential Monte Carlo (MC) filters [38], the posterior probability density function $P\left({\mathrm{x}}_{0:k}\right|{\mathrm{y}}_{1:k})$ is characterized by the set of support points ${\left\{{\mathrm{x}}_{0:k}^i\right\}}_{i=1}^{N_p}$ (or particles) and the corresponding weights ${\left\{w_k^i\right\}}_{i=1}^{N_p}$, where $N_p$ is the number of particles [39]. The posterior density at time k is approximated as

$$P\left({\mathrm{x}}_{0:k}\right|{\mathrm{y}}_{1:k})\approx \sum _{i=1}^{N_p}{w}_k^i\delta ({\mathrm{x}}_{0:k}-{\mathrm{x}}_{0:k}^i)$$

(14)

such that

$$\sum _{i=1}^{N_p}{w}_k^i=1,$$

(15)

where $\delta (·)$ represents the Dirac delta function. We assume that the particles are sampled from a well-known proposal distribution,

$${\mathrm{x}}_{0:k}^i\sim q\left({\mathrm{x}}_{0:k}^i\right|{\mathrm{y}}_{1:k}).$$

(16)

Then, by the principle of importance sampling, the corresponding weight is calculated as

$$w_k^i\propto \frac{P\left({\mathrm{x}}_{0:k}^i\right|{\mathrm{y}}_{1:k})}{q\left({\mathrm{x}}_{0:k}^i\right|{\mathrm{y}}_{1:k})}.$$

(17)

Because we have freedom to choose the proposal distribution, we consider a proposal distribution that has a form of

$$q\left({\mathrm{x}}_{0:k}\right|{\mathrm{y}}_{1:k})=q\left({\mathrm{x}}_k\right|{\mathrm{x}}_{0:k-1},{\mathrm{y}}_{1:k})q\left({\mathrm{x}}_{0:k-1}\right|{\mathrm{y}}_{1:k-1}).$$

(18)

In other words, one can draw new support points ${\mathrm{x}}_{0:k}^i\sim q\left({\mathrm{x}}_{0:k}\right|{\mathrm{y}}_{1:k})$ by augmenting each of the previous support points ${\mathrm{x}}_{0:k-1}^i\sim q\left({\mathrm{x}}_{0:k-1}\right|{\mathrm{y}}_{1:k-1})$ with the new state ${\mathrm{x}}_k^i\sim q\left({\mathrm{x}}_k\right|{\mathrm{x}}_{0:k-1},{\mathrm{y}}_{1:k})$.

Starting from Equation (17),

$$w_k^i\propto w_{k-1}^i\frac{P\left({\mathrm{x}}_{0:k}^i\right|{\mathrm{y}}_{1:k})}{P\left({\mathrm{x}}_{0:k-1}^i\right|{\mathrm{y}}_{1:k-1})}\frac{q\left({\mathrm{x}}_{0:k-1}^i\right|{\mathrm{y}}_{1:k-1})}{q\left({\mathrm{x}}_{0:k}^i\right|{\mathrm{y}}_{1:k})}.$$

(19)

Together with Equation (18) and the recursive relation (Equation (13)), the weight update equation can be simplified.

$$w_k^i\propto w_{k-1}^i\frac{P\left({\mathrm{y}}_k\right|{\lambda }_k^i,{\phi }_k^i,h_k^i)·P({\lambda }_k^i,{\phi }_k^i,v_{\lambda ,k}^i,v_{\phi ,k}^i|{\mathrm{x}}_{k-1}^i)·P\left(h_k^i\right|{\lambda }_k^i,{\phi }_k^i)·P\left(v_{h,k}^i\right|{\lambda }_k^i,{\phi }_k^i,v_{\lambda ,k}^i,v_{\phi ,k}^i)}{q\left({\mathrm{x}}_k^i\right|{\mathrm{x}}_{0:k-1}^i,{\mathrm{y}}_{1:k})}$$

(20)

A further assumption regarding the proposal distribution,

$$q\left({\mathrm{x}}_k^i\right|{\mathrm{x}}_{0:k-1}^i,{\mathrm{y}}_{1:k})=P({\lambda }_k^i,{\phi }_k^i,v_{\lambda ,k}^i,v_{\phi ,k}^i|{\mathrm{x}}_{k-1}^i)·P\left(h_k^i\right|{\lambda }_k^i,{\phi }_k^i)·P\left(v_{h,k}^i\right|{\lambda }_k^i,{\phi }_k^i,v_{\lambda ,k}^i,v_{\phi ,k}^i)$$

(21)

yields

$$w_k^i\propto w_{k-1}^i·P\left({\mathrm{y}}_k\right|{\lambda }_k^i,{\phi }_k^i,h_k^i)$$

(22)

This means that the weight of each particle is updated proportionally to its corresponding likelihood. Note that the above weight update equation implicitly includes the normalization given by Equation (15).

The proposed algorithm, STC-PF, is summarized in Algorithm 1. In a vanilla SIR PF, the next state is propagated through the dynamic model only. In contrast, in STC-PF, the next state is propagated through the dynamic model first and then propagated through the elevation model (line numbers 1 and 1 in Algorithm 1).

In Figure 3, a detailed implementation of the elevation model propagation is shown. It is worth mentioning that elevation model propagation can be accelerated by two techniques: parallelization and use of local data during the GP inference. Because the propagation process for each particle does not require information on other particles, it can be parallelized. Furthermore, during the GP inference, only the neighborhood data of DTED are utilized. The range of the neighborhood is defined by the spatial window size L.

Algorithm 1: Soft Terrain Constrained Particle Filter (STC-PF).

3.5. Remark on an Existing Work

As mentioned in Section 1, from a mathematical perspective, the proposed algorithm (STC-PF) is similar to scPF (soft-constrained Particle Filter) [35]. Similar to STC-PF, scPF is based on the SIR particle filter; however, the two differ in the sense that scPF utilizes generalized likelihood.

$${\widehat{w}}_k^i\propto w_{k-1}^{i}P\left({\mathrm{y}}_k\right|{\mathrm{x}}_k^i)P\left({\mathrm{C}}_k\right|{\mathrm{x}}_k^i)$$

(23)

where $P\left({\mathrm{C}}_k\right|{\mathrm{x}}_k^i)$ is a pseudo-measurement that represents how much the given state ${\mathrm{x}}_k^i$ satisfies the constraint. If Equation (21) is replaced by

$$q\left({\mathrm{x}}_k^i\right|{\mathrm{x}}_{0:k-1}^i,{\mathrm{y}}_{1:k}^i)=P({\lambda }_k^i,{\phi }_k^i,v_{\lambda ,k}^i,v_{\phi ,k}^i|{\mathrm{x}}_{k-1}^i),$$

(24)

then the weight update rule is also changed.

$$w_k^i\propto w_{k-1}^i·P\left({\mathrm{y}}_k\right|{\lambda }_k^i,{\phi }_k^i,h_k^i)·P\left(h_k^i\right|{\lambda }_k^i,{\phi }_k^i)·P\left(v_{h,k}^i\right|{\lambda }_k^i,{\phi }_k^i,v_{\lambda ,k}^i,v_{\phi ,k}^i)$$

(25)

Thus, the generalized likelihood function can be identified by equating the elevation model with the pseudo-measurement. As a result, scPF can be reduced to STC-PF as long as the assumption for target motion holds.

4. Simulation

4.1. Scenario and Parameter Settings

To evaluate STC-PF, numerical experiments are performed with the following scenario: The radar is mounted on an aircraft that flies at a speed of 70 m/s at a height of 2500 m. The radar tracks a single target that moves along the surface at a speed of 25 m/s. (see Figure 4) The simulation runs for 100 s. Furthermore, to reflect the uncertainty in DTED, a noisy version of DTED is created. More specifically, iid zero-mean Gaussian noise with variance ${\sigma }_{\mathrm{DTED}}$ is sampled and added for each data entry in DTED. Because it is reasonable to bound the uncertainty of DTED, sampled noise is clipped to 50 m if its absolute value exceeds 50 m.

Values of parameters used in the simulation are listed in

Table 1. Parameter Setting.

Name	Value
${\sigma }_{\mathrm{DTED}}\left(m\right)$	$3.77$, $1.89$
$\alpha$	100
$\mathrm{\Gamma }\left(de{g}^{-2}\right)$	$\mathrm{diag}\left[\begin{array}{cc}\frac{1}{{(2.78e-4)}^2}& \frac{1}{{(2.78e-4)}^2}\end{array}\right]$
$L\left(arcsec\right)$	$13(\approx 390m)$
$\Delta t$$\left(s\right)$	$1.0$
Initial Cov.	$\left[\begin{array}{cc}1e2\left(m^2\right)·{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& 10(m^2/s^2)·{\mathbf{I}}_{3\times 3}\end{array}\right]$
$N_p$	$1e4$
Q	${\left[\begin{array}{cccc}20\left(m\right)·{\mathbf{I}}_{3\times 3}& \phantom{0}& {\mathbf{0}}_{3\times 3}& \phantom{0}\\ \phantom{0}& 2(m/s)& 0& 0\\ {\mathbf{0}}_{3\times 3}& 0& 2(m/s)& 0\\ \phantom{0}& 0& 0& 5(m/s)\end{array}\right]}^2$
R	${\left(\mathrm{diag}\left[\begin{array}{ccc}10\left(m\right)& 0.1\left(deg\right)& 0.1\left(deg\right)\end{array}\right]\right)}^2$

. Detailed explanation about the choice of GP hyper-parameters is in the Appendix B. The simulations are performed with two settings that differ in the value of ${\sigma }_{\mathrm{DTED}}$. The reasonable value for ${\sigma }_{\mathrm{DTED}}$ is 3.77 m, which is inferred from [37]. However, another setting whose ${\sigma }_{\mathrm{DTED}}$ is 1.89 m is also used to observe the sensitivity of the key parameter.

4.2. Baseline Methods

To compare STC-PF with other filters that can incorporate nonlinear constraints, the Smoothly Constrained Kalman Filter (SCKF) is implemented as well [30]. Note that ‘Smoothly Constrained’ in the name of SCKF does not mean soft constraint. Because SCKF can incorporate only deterministic constraints, it requires approximations of ground-truth terrain elevation that require h and $\nabla h$ to be fixed to specific values. One approach used for the comparison is to ignore the noise inherent in DTED and use bilinear interpolation to retrieve the terrain elevation at arbitrary positions.

$$\begin{array}{c}E\left({\mathrm{x}}_k\right)=h_k-\overline{h}({\lambda }_k,{\phi }_k)=0\hfill \\ E_v\left({\mathrm{x}}_k\right)=v_{h,k}-\nabla \overline{h}({\lambda }_k,{\phi }_k)·{\left[\begin{array}{cc}{v}_{\lambda ,k}& v_{\phi ,k}\end{array}\right]}^T=0\hfill \\ \overline{h}(\lambda ,\phi )\approx BL(\lambda ,\phi |\mathrm{DTED})\hfill \end{array}$$

(26)

where $BL(\lambda ,\phi |\mathrm{DTED})$ is bilinear interpolation at $(\lambda ,\phi )$ given $\mathrm{DTED}$. For the computation of the gradient $\nabla \overline{h}$, central numerical differentiation is used instead of analytic differentiation to avoid non-differentiable cases.

$${\left.\frac{\partial \overline{h}}{\partial \lambda }\right|}_{{\mathrm{x}}_k}\approx \frac{\overline{h}({\lambda }_k+\Delta ,{\phi }_k)-\overline{h}({\lambda }_k-\Delta ,{\phi }_k)}{2\Delta }$$

(27)

where $\Delta$ is a small constant.

Another method is to use GP mean regression rather than bilinear interpolation. That is,

$${\left[\begin{array}{cc}\overline{h}& \nabla \overline{h}\end{array}\right]}^T\approx \overline{\mu }$$

(28)

where $\overline{\mu }$ is the GP joint mean of $\overline{h}$ and $\nabla \overline{h}$ in Equation (9). This enables us to reconstruct the most probable ground-truth terrain elevation considering the noise of DTED; however, this method still cannot consider the uncertainty of the inferred $\overline{h}$ and $\nabla \overline{h}$ values, in contrast to STC-PF.

SCKF requires the Jacobian of the constraint functions:

$$\begin{array}{ccc}G\left({\mathrm{x}}_k\right)=& {\left.\frac{\partial E}{\partial \mathrm{x}}\right|}_{{\mathrm{x}}_k}=& {\left.\left[\begin{array}{cccccc}\frac{\partial E}{\partial x}& \frac{\partial E}{\partial y}& \frac{\partial E}{\partial z}& \frac{\partial E}{\partial v_x}& \frac{\partial E}{\partial v_y}& \frac{\partial E}{\partial v_z}\end{array}\right]\right|}_{{\mathrm{x}}_k}\\ \\ G_v\left({\mathrm{x}}_k\right)=& {\left.\frac{\partial E_v}{\partial \mathrm{x}}\right|}_{{\mathrm{x}}_k}=& {\left.\left[\begin{array}{cccccc}\frac{\partial E_v}{\partial x}& \frac{\partial E_v}{\partial y}& \frac{\partial E_v}{\partial z}& \frac{\partial E_v}{\partial v_x}& \frac{\partial E_v}{\partial v_y}& \frac{\partial E_v}{\partial v_z}\end{array}\right]\right|}_{{\mathrm{x}}_k}\end{array}$$

(29)

However, it is impossible to differentiate $E\left({\mathrm{x}}_k\right)$ and $E_v\left({\mathrm{x}}_k\right)$ analytically because they involve coordinate transformation between local Cartesian and WGS84 LLA. Alternatively, the derivative can be obtained using the central numerical difference regardless of the regression method.

$${\left.\frac{\partial E}{\partial x}\right|}_{{\mathrm{x}}_k}\approx \frac{E({\mathrm{x}}_k+\Delta ·e_x)-E({\mathrm{x}}_k-\Delta ·e_x)}{2\Delta },$$

(30)

where $e_x$ is a canonical unit vector whose first component is nonzero. $\partial E/\partial y_k$, $\partial E/\partial z_k$, and $\partial E_v/\partial \circ$ can be obtained in a similar way. Because E is not a function of $v_{·,k}$, corresponding derivatives automatically become zero.

4.3. Results

To evaluate STC-PF, SCKF using bilinear regression, and SCKF using GP mean regression, 100 Monte-Carlo simulations were carried out for each ${\sigma }_{\mathrm{DTED}}$ value. Tracking performance is assessed based on timewise RMS (Root Mean Squared) error. For example, timewise RMS for local Cartesian x position error at time k is

$$RM{S}_{x,k}=\sqrt{\frac{1}{N_{MC}}\sum _{n=1}^{N_{MC}}{(x_k^n-{\overline{x}}_k)}^2}$$

(31)

where $N_{MC}$ is the number of repetitions (i.e., 100), $x_k^n$ the filter mean value for x position at time k in the $n^{th}$ trial, and ${\overline{x}}_k$ the ground-truth x position at time k. The time average ($10\le k\le 90$) for timewise RMS is also computed for evaluation.

Figure 5 shows the timewise RMS for local Cartesian position error and velocity error. In the figures, SCKF using bilinear regression shows the worst tracking performance. In terms of time average of RMS position error, as shown in

Table 2. Time Average of Timewise RMS (${\sigma }_{\mathrm{DTED}}=3.77$ m).

		SCKF	SCKF
	STC-PF	+	+
		Bilinear	GP
$x$(m)	9.61	10.9	9.52
y	20.7	34.1	22.4
z	2.77	3.84	3.05
Position	23.0	36.1	24.6
$v_x$(m/s)	0.972	4.10	1.55
$v_y$	1.74	14.0	5.45
$v_z$	1.78	4.16	2.15
Velocity	2.75	15.4	6.15

, the superiority of STC-PF over SCKF using GP mean regression is clear, although it cannot be identified in Figure 5. In terms of RMS velocity error, STC-PF distinctly outperforms the other two methods. This trend also holds for the different parameter setting, namely ${\sigma }_{\mathrm{DTED}}$ = 1.89 m, as shown in Figure 6 and

Table 3. Time Average of Timewise RMS (${\sigma }_{\mathrm{DTED}}=1.89$ m).

		SCKF	SCKF
	STC-PF	+	+
		Bilinear	GP
x (m)	9.48	11.0	9.63
y	20.5	34.4	23.1
z	2.56	3.96	3.12
Position	22.8	36.4	25.3
$v_x$(m/s)	0.966	3.38	1.11
$v_y$	1.71	14.2	5.97
$v_z$	1.74	3.95	2.22
Velocity	2.69	15.4	6.56

.

On the other hand, the speed of the algorithms is assessed based on the average processing time for a single timestep. STC-PF and SCKF both were implemented in MATLAB and run on an Intel Core i7-8565U with 16.0GB RAM.

Table 4. Average Processing Time for a Single Timestep.

	STC-PF	SCKF	SCKF	STC-PF
	+	+	+	+
	Single	Bilinear	GP	Parallel
$t\left(s\right)$	43.5	1.18	1.33	14.2

shows that the baseline method runs nearly in real time, while STC-PF does not. Nevertheless, with parallel computing, STC-PF gets much faster and shows the possibility of real time applications.

4.4. Discussion

According to the simulation results, the tracking performance of SCKF depends on the regression method. More specifically, SCKF using GP mean regression has smaller RMS errors than SCKF using bilinear regression. This result could be due to the regression method used for target trajectory generation: GP regression, which is suitable for fitting of a smooth curve, might result in a trajectory closer to the ground-truth trajectory generated by bicubic spline interpolation.

Meanwhile, the superiority of STC-PF over SCKF in terms of tracking performance could stem from two factors. The first is that a particle filter is more expressive than a simple Gaussian filter. Because SCKF assumes that the posterior distribution is a simple Gaussian, SCKF adjusts its state to meet the constraint at one point. However, unlike SCKF, particle filters do not assume the form of a posterior distribution. Therefore, STC-PF can estimate the distribution of the velocity hypothesis for each position hypothesis, so that the combined hypothesis independently meets the terrain constraint. As a result, the filter mean value, a weighted sum of each hypothesis, is less biased.

The second reason is that the state estimation with soft constraint is less sensitive to the uncertainty of the constraint. Conversely, state estimation with hard constraint is very sensitive to the uncertainty of the constraint. Figure 7 shows that the terrain constraint (Equation (5)) holds almost perfectly assuming perfect knowledge of the position and DTED. However, the hard constraint, the GP mean regression, deviates from the ground-truth value if small amounts of position uncertainty (approximately 30 m in the longitudinal direction) and DTED noise (${\sigma }_{\mathrm{DTED}}=3.77$ m) are introduced (see Figure 8). In other words, uncertainty of the horizontal position and DTED can result in a catastrophic state error if a hard constraint is applied. On the other hand, STC-PF can absorb the error to some degree as most of the ground-truth value resides inside the two-sigma bound.

Regarding the speed of the algorithm, it is natural that STC-PF consumes more computational resources than SCKF. This is because STC-PF requires expensive computation for every particle; GP inference whose time complexity is $\mathcal{O}\left(n^3\right)$, where n is the number of observation points involved in the GP inference. Fortunately, time consumption is drastically reduced by the virtue of parallel computing, and it can be reduced further by two measures. One is reducing the number of GP observation points (namely, the spatial window L in Figure 3) as far as the performance degradation is negligible, and the other is increasing the number of cores dedicated to the filtering.

5. Conclusions

To sum up, we have proposed a particle filter to improve the performance of ground target tracking. To estimate the velocity more accurately, not only a position constraint but also a velocity constraint has been introduced in the terrain constraint. Although DTED provides terrain elevation of the entire globe, it provides inaccurate values at discrete positions. Thus, the ground-truth terrain elevation included in the terrain constraint has been modeled with a Gaussian process, and DTED has been regarded as noisy observations of it. As a result, terrain constraint has become a soft constraint that can reflect the uncertainty of DTED. Finally, we have proposed a particle filter, STC-PF, given the assumption of the motion of the target. STC-PF is based on SIR PF, but the major difference is that STC-PF uses the elevation model. Due to the elevation model, knowledge of the horizontal position and velocity of a target enables us to infer the vertical position and velocity more precisely. In the numerical simulation, STC-PF has been compared with SCKF which can incorporate hard constraints only. Furthermore, to reflect the uncertainty in DTED, filters have made use of DTED contaminated by noise, whereas the ground-truth trajectory of the target is generated by the original DTED. The simulation results showed that STC-PF outperforms SCKF in terms of RMS error, for two possible reasons. The first is that particle filters are more expressive than simple Gaussian filters. The second is that the state estimation with soft constraint is less sensitive to uncertainty of the constraint than that with hard constraint.