Angle-Only Filtering of a Maneuvering Target in 3D

Abstract

We consider the state estimation of a maneuvering target in 3D using bearing and elevation measurements from a passive infrared search and track (IRST) sensor. Since the range is not observable, the sensor must perform a maneuver to observe the state of the target. The target moves with a nearly constant turn (NCT) in the $XY$-plane and nearly constant velocity (NCV) along the Z-axis. The natural choice for the NCT motion is to allow perturbations in speed and angular rate in the stochastic differential equation, as has been pointed out previously for a 2D scenario using range and bearing measurements. The NCT motion in the $XY$-plane cannot be discretized exactly, whereas the NCV motion along the Z-axis is discretized exactly. We discretize the continuous-time NCT model using the first and second-order Taylor approximations to obtain discrete-time NCT models, and we consider the polar velocity and Cartesian velocity-based states for the NCT model. The dynamic and measurement models are nonlinear in the target state. We use the cubature Kalman filter to estimate the target state. Accuracies of the first and second-order Taylor approximations are compared using the polar velocity-based and Cartesian velocity-based models using Monte Carlo simulations. Numerical results for realistic scenarios considered show that the second-order Taylor approximation provides the best accuracy using the polar velocity or Cartesian velocity-based models.

1. Introduction

Angle-only filtering in 2D and 3D finds many important applications in passive tracking [1,2,3,4,5,6,7,8,9,10,11,12,13]. The advantage of passive tracking over active tracking is that the presence of the passive sensor cannot be detected by the target. Passive tracking arises in submarine tracking using a passive sonar [1,11], passive ranging using an infrared search and track (IRST) sensor [2,4,5,8,12], passive radar tracking [4], satellite-to-satellite passive tracking [14], video tracking [15], etc. In this paper, we focus on tracking an aircraft using an IRST sensor on another aircraft. This problem is more difficult than the case where multiple sensors are used. The bearings-only filtering (BOF) problem in 2D has been extensively studied, and a vast number of publications exist in the research literature [1,10,16,17,18,19], Chapter 6 of [11,20]. However, research in the angle-only filtering (AOF) problem in 3D is limited compared to that in the bearings-only filtering problem.

Observability is a major issue for the BOF problem in 2D [21] and AOF problem in 3D. In the 2D problem, a four or five-dimensional state is estimated from bearings-only measurements for a non-maneuvering and maneuvering target, respectively. To observe the state of the target, the sensor must perform maneuvers with a motion of higher order than that of the target [18]. If a four-dimensional Cartesian state is used for the BOF problem in 2D for a non-maneuvering target, it has been observed that the extended Kalman filter (EKF) [22,23] diverges. Modified polar coordinates (MPC) [1,24,25] were formulated to overcome the divergence of the EKF. The components of the MPC are bearing, bearing-rate, range-rate divided by range, and the inverse of range [1]. The first three components of MPC are observable even before an ownship maneuver. MPC decouple the observable and unobservable components of the state vector and provide improved performance of the EKF. Use of MPC makes the dynamic model for the nearly constant velocity (NCV) model nonlinear and complex, but the measurement model becomes linear. In addition to the EKF, the unscented Kalman filter (UKF) [26], cubature Kalman filter (CKF) [27], Gaussian sum filter (GSF) [28], particle filter (PF) [11], uncorrelated conversion based filter (UCF) [20], etc. have also been applied to the BOF problem in 2D. In order to address the observability problem, the multiple model-based range-parametrized (RP) EKF (RP-EKF) was proposed by Peach [19] and Kronhamn [16]. In addition to the EKF, other filters can also be used in the RP framework.

Most of the existing work on the angle-only filtering problem in 3D is for a non-maneuvering target using the NCV model. The sensor must perform a maneuver to observe the target state. For a non-maneuvering target, the EKF using the Cartesian state for the AOF problem in 3D does not diverge [8], even though the filter diverges for the corresponding BOF in 2D [11]. In analogy with the MPC in 2D, Stallard proposed the modified spherical coordinates (MSC) in 3D [12,13]. The components of the MSC are elevation, elevation-rate, bearing, bearing-rate times cosine of elevation, the inverse of range, and range-rate divided by range. As in the case of MPC, the dynamic model for the NCV motion using MSC is nonlinear and complex. However, the measurement model is linear since bearing and elevation are components of MSC. The log spherical coordinates (LSC) [29] can also be used as an alternate to the MSC. The first five components of the LSC are the same as those of the MSC, but the inverse of range (sixth component) is replaced by the logarithm of the range. Many studies have shown that the EKF using the MSC (EKF-MSC) provides a better state estimation accuracy than the Cartesian EKF (CEKF) for the NCV motion [2,12,13].

Starting with the work of Stallard, the EKF-MSC was used in [2,12,13,30]. Karlsson and Gustafsson used the PF and compared the PF-based algorithms with the multiple model-based range-parametrized EKF (RP-EKF) using the Cartesian state and MSC in a number of tracking scenarios [6,7]. Their results showed the superiority of the PF-based algorithms over the RP-EKF-based algorithms.

In our previous work [29], we compared the EKF-MSC and EKF-LSC using the continuous-discrete filtering approach with the discrete-time CEKF. The results of this study show that the EKF-MSC and EKF-LSC have comparable accuracy and perform better than the discrete-time CEKF for low measurement accuracy. For high measurement accuracy, the discrete-time CEKF has higher state estimation accuracy than the EKF-MSC and EKF-LSC. Prior to our work in [8,31,32], the process noise using the MSC was modeled approximately. We proposed new algorithms using the MSC to model the process noise exactly. The AOF for the NCV motion can be solved in the following three possible ways [8,31,32]:

• Use the discrete-time NCV model with the Cartesian state vector (with linear dynamic model) and nonlinear measurement model;
• Use the exact discrete-time NCV model with the MSC (with nonlinear dynamic model) and linear measurement model;
• Use the MSC with approximate discretization of the continuous-time dynamic model (with nonlinear dynamic model) and linear measurement model.

In [8], we performed a comprehensive study of the AOF problem for a non-maneuvering target in 3D using the EKF, UKF, and PF with Cartesian state vector and MSC. In this study, new algorithms using the EKF, UKF, and PF with the MSC were formulated, and improved filter initialization algorithms for the Cartesian state and MSC were presented. Four versions of the PF were used in this work: the Cartesian bootstrap filter (CBF), bootstrap filter using MSC with exact dynamic model (BF-MSC(E)), bootstrap filter using MSC with an approximate dynamic model (BF-MSC(A)), and the optimal importance density-based PF using MSC with an approximate dynamic model (ODIPF-MSC(A)). The initial range between the target and the sensor in the scenarios in [8] is higher than that in [6,7]. Numerical results from this study indicate that the state estimation accuracy of the PF-based algorithms is inferior compared with that of the EKF and UKF-based algorithms. For the BOF problem in 2D Chapter 6 of [11], the measurement SD is of the order of a degree and the measurement time interval is about 30–60 s. In this scenario, a PF has one of the best state estimation accuracies Chapter 6 of [11,20]. Secondly, compared to the EKF, the PF-based algorithms have about two orders of magnitude higher computational cost. Thirdly, It is now well established that, when the measurement accuracy and data rate are high (which is true for the current problem), PFs do not offer any advantage over the EKF, UKF, and CKF [33,34,35]. Therefore, we did not consider the PF in this study.

A novel batch Bayesian weighted instrumental variable estimator for the 3D target motion analysis problem using bearing and elevation measurements is presented in [36]. Results of this study show that the proposed algorithm outperforms its non-Bayesian counterpart. The CEKF, Cartesian UKF (CUKF), Cartesian CKF (CCKF), and the Cartesian new sigma point Kalman filter (CNSKF) were used in [3] to analyze the AOF problem in 3D for a non-maneuvering target. Results of this study shows that these five filters have nearly the same accuracy in operational scenarios. The particle flow filter (PFF), ensemble Kalman filter (EnKF), EKF, UKF, and PF were compared for the AOF problem in 3D for a non-maneuvering target in [37]. It was observed that the EKF-MSC, UKF-MSC, deterministic EnKF-MSC, and Cartesian PFF had the best performance in operational conditions.

In [38], we studied the passive sonar tracking problem when the submarine and the ownship move in different planes using the EKF, UKF, RP-UKF, and PF. Our results showed that the depth of the non-maneuvering target can be estimated accurately, and the PF had the best performance in the scenarios studied. The 3D instrumental variable-based Kalman filter (3D-IVKF) is applied to an underwater passive sonar tracking scenario in [39] for a non-maneuvering target using bearing and elevation measurements. It is observed that at low measurement standard deviations (SDs) (<6°) the performance of the 3D-IVKF is comparable to that of the UKF and CKF. However, at higher measurement SDs, the 3D-IVKF outperforms the UKF and CKF with lower computational cost.

To compare the accuracies of the filters used in the AOF problem with the best achievable accuracy, we computed the posterior Cramér-Rao lower bound (PCRLB) [40] for a non-maneuvering target using the NCV model in [41]. Our results show that when the measurement accuracy is high, the root mean square (RMS) position and velocity errors are close to the corresponding PCRLBs. The difference between RMS position and velocity errors and corresponding PCRLBs increases with a decrease in the measurement accuracy. In [42], a globally valid posterior Cramér–Rao lower bound was derived for the AOF problem. The authors claim the von Mises–Fisher distribution to be superior to the conventional approach using additive Gaussian noise in measured angular coordinates.

A maneuvering target refers to an accelerating target [43]. Common accelerated motions considered in tracking are the nearly constant acceleration (NCA), nearly constant turn (NCT), and jerk [4,22,43]. The NCA and jerk models are linear, whereas the NCT model is nonlinear in the target state. The number of publications for a maneuvering target in the AOF problem is quite limited. In [5], the NCT model was used in the passive ranging problem using an IRST sensor in air-to-air tracking scenarios. The authors used the RP-UKF using the multiple model method. However, algorithm details are not presented in the paper. The NCT model in the $XY-$plane has been studied extensively where the angular rate is estimated [4,22,43,44,45,46]. This problem arises in the air-traffic control (ATC) scenario [4,22,27,43,47]. In most cases, the conventional discrete-time NCT model is approximate, since the state transition matrix and process noise covariance matrix cannot be derived from the continuous-time model using a consistent procedure.

We consider the tracking of a maneuvering aircraft in 3D and assume that the aircraft moves in the $XY$-plane with the NCT motion and has a NCV motion along the Z-axis. The speed and angular rate are constant for the constant turn motion (CT) in the $XY$-plane. Thus, it is natural to perturb the speed and angular rate in the NCT motion with the continuous-time white noise (Wiener processes) [22]. We follow this approach from [45] to obtain the nonlinear stochastic differential equation (SDE) [48,49] for the NCT motion. Since the SDE is nonlinear, it cannot be discretized exactly. We discretize the SDE using the first and second-order weak Taylor (TS2) approximations [50] to obtain approximate discrete-time dynamic models. The first-order stochastic Taylor series approximation is also known as the Euler approximation. Two types of states for the NCT motion in the $XY$-plane, namely the polar velocity and the Cartesian velocity-based states [43,44,45,46], are used. The NCV motion along the Z-axis is discretized exactly. The Cartesian velocity state in NCT comprises the 2D position, 2D velocity, and angular rate. In the polar velocity state, the speed and heading replace the 2D Cartesian velocity.

An IRST sensor on another maneuvering aircraft collects azimuth and elevation measurements. The accuracy of the angle measurements by an IRST sensor is usually high (1 mrad). The data rate of an IRST sensor is also high (1 Hz). As sensor technology improves, these factors are expected to improve. The AOF algorithm is required to process the sensor measurements sequentially in real time. Thus, a batch algorithm is ruled out for this tracking scenario. As discussed previously, a PF is not considered for this problem due to its lack of state estimation accuracy and high computational cost. It has been observed in [33,34,35] that when the measurement accuracy and data rate are high (which is true for the current problem), the UKF and CKF have nearly the same accuracy, and the accuracy of the EKF is somewhat lower. If the dimension of the state is n, then the UKF and CKF have $2n+1$ and $2n$ sigma points and cubature points, respectively. As a result, the computational cost of the CKF is lower than that of the UKF. If $n>3$, then the first weight in the UKF becomes negative and the rest of the $2n$ weights are positive. On the contrary, each of the $2n$ weights in the CKF is positive and equal to $1/2n$. This negative weight may cause a filter-calculated covariance matrix to be non-positive definite in some cases [27]. The CKF was also successfully used in our previous work on AOF in [9]. Hence, we chose the third-degree spherical–radial cubature rule-based CKF [27] to estimate the seven-dimensional state of the maneuvering target. The CKFs using the Euler and TS2 approximations are called CKF1 and CKF2, respectively. Thus, we consider four CKF filters; CKF1P, CKF1C, CKF2P, and CKF2C, where the last letter in the filter refers to polar and Cartesian velocity states.

Notation convention: For clarity, we use italics to denote scalar quantities and boldface for vectors and matrices. A lower or upper-case Roman letter represents a name (e.g., “s” for “sensor,” “RMS” for “root mean square,” etc.). We use “:=” to define a quantity and ${\mathbf{A}}^{\prime }$ denotes the transpose of the vector or matrix $\mathbf{A}$. The $n-$dimensional identity matrix, $m-$dimensional null matrix, and $m\times n$ null matrix are denoted by ${\mathbf{I}}_n$, ${\mathbf{0}}_m$, and ${\mathbf{0}}_{m\times n}$, respectively.

The paper is organized as follows. Section 2 presents the dynamic models for the target. Section 3 explains the discretization of target NCT models, and Section 4 describes sensor dynamic and measurement models. A summary of the four CKF filters is given in Section 5. Numerical simulations and results are presented in Section 6. Finally, Section 7 summarizes our contributions in the paper.

2. Target Dynamic Models

We assume that the IRST sensor trajectory is deterministic and the states of the sensors are known exactly at measurement times. To improve the observability of the target state, the IRST sensor performs maneuvers with a sequence of CV and constant turn (CT) motions [5,8,31].

Two types of coordinates are commonly used for the NCT in the $XY$-plane: Cartesian velocity and polar velocity-based models [43,44,45,46]. In addition to the 2D position and velocity, the turn-rate or angular velocity $\omega$ is also estimated in the NCT model.

Let $\mathbf{z}\left(t\right)$ denote the Cartesian state along the Z-axis with position and velocity components

$$\mathbf{z}\left(t\right):={\left[\begin{array}{cc}z\left(t\right)& \dot{z}\left(t\right)\end{array}\right]}^{\prime }.$$

(1)

For the NCT model in the $XY$-plane, we use $\mathbf{\eta }\left(t\right)$ and $\mathbf{\xi }\left(t\right)$ for state vectors where the angular velocity $\omega$ is estimated. The velocity in $\mathbf{\eta }\left(t\right)$ and $\mathbf{\xi }\left(t\right)$ has Cartesian and polar coordinates, respectively. Let $s\left(t\right)$ and $\theta \left(t\right)$ denote the speed and heading of the target in the $XY$-plane. In this paper, the heading is defined as the angle of the velocity in the $XY$-plane, measured from the X-axis in the counter-clockwise direction, as shown in Figure 1.

Then $\mathbf{\eta }\left(t\right)$ and $\mathbf{\xi }\left(t\right)$ are defined, respectively, by

$$\mathbf{\eta }\left(t\right):={\left[\begin{array}{ccccc}x\left(t\right)& y\left(t\right)& \dot{x}\left(t\right)& \dot{y}\left(t\right)& \omega \left(t\right)\end{array}\right]}^{\prime },$$

(2)

$$\mathbf{\xi }\left(t\right):={\left[\begin{array}{ccccc}x\left(t\right)& y\left(t\right)& s\left(t\right)& \theta \left(t\right)& \omega \left(t\right)\end{array}\right]}^{\prime }.$$

(3)

Three-dimensional state vectors where angular velocity is estimated are defined, respectively, by

$${\mathbf{x}}_{\mathrm{c}}\left(t\right):={\left[\begin{array}{cc}\mathbf{\eta }{\left(t\right)}^{\prime }& \mathbf{z}{\left(t\right)}^{\prime }\end{array}\right]}^{\prime },$$

(4)

$${\mathbf{x}}_{\mathrm{p}}\left(t\right):={\left[\begin{array}{cc}\mathbf{\xi }{\left(t\right)}^{\prime }& \mathbf{z}{\left(t\right)}^{\prime }\end{array}\right]}^{\prime }.$$

(5)

We assume that the measurement time interval is constant; i.e., $t_k-t_{k-1}=T$ for all k. In this paper, we use the discretized continuous-time models [22].

The discrete-time dynamic model for the NCV motion along the Z-axis is given by

$${\mathbf{z}}_k={\mathbf{F}}_1{\mathbf{z}}_{k-1}+{\mathbf{w}}_{\mathbf{z},k-1},$$

(6)

where ${\mathbf{F}}_1$ is the state transition matrix and ${\mathbf{w}}_{\mathbf{z},k-1}$ is a zero-mean white Gaussian process noise with covariance ${\mathbf{Q}}_{\mathbf{z}}$,

$${\mathbf{F}}_1=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right],$$

(7)

$${\mathbf{Q}}_{\mathbf{z}}=q_z\mathbf{B},$$

(8)

$$\mathbf{B}=\left[\begin{array}{cc}{T}^3/3& T^2/2\\ T^2/2& T\end{array}\right],$$

(9)

where $q_z$ is the power spectral density (PSD) of the continuous-time acceleration process noise along the Z-axis [22].

The time derivative of $\mathbf{\xi }\left(t\right)$ is given by

$$\dot{\mathbf{\xi }}\left(t\right)={\left[\begin{array}{ccccc}\dot{x}\left(t\right)& \dot{y}\left(t\right)& \dot{s}\left(t\right)& \dot{\theta }\left(t\right)& \dot{\omega }\left(t\right)\end{array}\right]}^{\prime }.$$

(10)

We have

$$\dot{x}\left(t\right)=s\left(t\right)cos\theta \left(t\right),\dot{y}\left(t\right)=s\left(t\right)sin\theta \left(t\right).$$

(11)

Since $\dot{\theta }\left(t\right)=\omega \left(t\right)$, (10) can be written as

$$\dot{\mathbf{\xi }}\left(t\right)={\left[\begin{array}{ccccc}s\left(t\right)cos\theta \left(t\right)& s\left(t\right)sin\theta \left(t\right)& \dot{s}\left(t\right)& \omega \left(t\right)& \dot{\omega }\left(t\right)\end{array}\right]}^{\prime }.$$

(12)

The time derivative of $\mathbf{\eta }\left(t\right)$ is

$$\dot{\mathbf{\eta }}\left(t\right)={\left[\begin{array}{ccccc}\dot{x}\left(t\right)& \dot{y}\left(t\right)& \ddot{x}\left(t\right)& \ddot{y}\left(t\right)& \dot{\omega }\left(t\right)\end{array}\right]}^{\prime }.$$

(13)

In the constant turn (CT) model, the speed and turn rate are constant. The speed and turn rate can be modeled as nearly constant in the NCT motion. Examining (12) and (13), we find that for the NCT model, (12) is more suitable than (13), based on symmetry considerations. Using conventional models in the engineering literature [22], for the NCT model, we may write

$$\dot{s}\left(t\right)=w_s\left(t\right),\dot{\omega }\left(t\right)=w_{\omega }\left(t\right),$$

(14)

where $w_s\left(t\right)$ and $w_{\omega }\left(t\right)$ are continuous-time zero-mean white acceleration and angular acceleration process noises with power spectral densities $q_s$ and $q_{\omega },$ respectively, [22]

$$E\left\{w_s\left(t\right)\right\}=0,E\left\{w_s\left(t\right)w_s\left(\tau \right)\right\}=q_s\delta (t-\tau ),$$

(15)

$$E\left\{w_{\omega }\left(t\right)\right\}=0,E\left\{w_{\omega }\left(t\right)w_{\omega }\left(\tau \right)\right\}=q_{\omega }\delta (t-\tau ),$$

(16)

where $\delta$ is the Dirac delta function [51]. We can write (14)–(16) mathematically rigorously by defining

$$ds\left(t\right)=\sqrt{q_s}d{\beta }_s\left(t\right),d\omega \left(t\right)=\sqrt{q_{\omega }}d{\beta }_{\omega }\left(t\right),$$

(17)

where $d{\beta }_s\left(t\right)$ and $d{\beta }_{\omega }\left(t\right)$ are standard independent Wiener processes [45,48]

$$E\left\{d{\beta }_s\left(t\right)d{\beta }_s\left(t\right)\right\}=dt,E\left\{d{\beta }_{\omega }\left(t\right)d{\beta }_{\omega }\left(t\right)\right\}=dt,$$

(18)

$$E\left\{d{\beta }_s\left(t\right)d{\beta }_{\omega }\left(t\right)\right\}=0.$$

(19)

Define

$${\mathbf{f}}_{\mathrm{p}}\left(\mathbf{\xi }\left(t\right)\right):={\left[\begin{array}{ccccc}s\left(t\right)cos\theta \left(t\right)& s\left(t\right)sin\theta \left(t\right)& 0& \omega \left(t\right)& 0\end{array}\right]}^{\prime },$$

(20)

$${\mathbf{w}}_{\mathrm{p}}\left(t\right):={\left[\begin{array}{ccccc}0& 0& w_s\left(t\right)& 0& w_{\omega }\left(t\right)\end{array}\right]}^{\prime },$$

(21)

$${\mathbf{G}}_{\mathrm{p}}:={\left[\begin{array}{ccccc}0& 0& \sqrt{q_s}& 0& 0\\ 0& 0& 0& 0& \sqrt{q_{\omega }}\end{array}\right]}^{\prime },$$

(22)

$$d\mathbf{\beta }\left(t\right):={\left[\begin{array}{cc}d{\beta }_s\left(t\right)& d{\beta }_{\omega }\left(t\right)\end{array}\right]}^{\prime }.$$

(23)

Then, conventionally, we can write $\dot{\mathbf{\xi }}\left(t\right)$ as [23,48]

$$\dot{\mathbf{\xi }}\left(t\right)={\mathbf{f}}_{\mathrm{p}}\left(\mathbf{\xi }\left(t\right)\right)+{\mathbf{w}}_{\mathrm{p}}\left(t\right).$$

(24)

We can write the time derivative of the polar state vector mathematically rigorously using the Itô stochastic differential equation (SDE) [45,48,49]

$$d\mathbf{\xi }\left(t\right)={\mathbf{f}}_{\mathrm{p}}\left(\mathbf{\xi }\left(t\right)\right)dt+{\mathbf{G}}_{\mathrm{p}}d\mathbf{\beta }\left(t\right),$$

(25)

where

$$E\left\{d\mathbf{\beta }\left(t\right)d{\mathbf{\beta }}^{\prime }\left(t\right)\right\}={\mathbf{I}}_{2}dt.$$

(26)

We assume that the prior distribution of $\mathbf{\xi }$ is Gaussian,

$${\mathbf{\xi }}_0=\mathbf{\xi }\left(t_0\right)\sim \mathcal{N}({\mathbf{\xi }}_0;{\overline{\mathbf{\xi }}}_0,{\mathbf{P}}_0^{\mathbf{\xi }}).$$

(27)

The time derivative of $\mathbf{\eta }$ contains Cartesian accelerations $\ddot{x}$ and $\ddot{y}$ in (13). It is necessary to transform them to derivatives of speed and angular velocity. The 2D Cartesian velocity is given by

$$\mathbf{v}\left(t\right)={\left[\begin{array}{cc}\dot{x}\left(t\right)& \dot{y}\left(t\right)\end{array}\right]}^{\prime }$$

(28)

and the Cartesian acceleration is $\dot{\mathbf{v}}\left(t\right).$ Using (11), the Cartesian acceleration is expressed by

$$\dot{\mathbf{v}}\left(t\right)={\left[\begin{array}{cc}\dot{s}\left(t\right)cos\theta \left(t\right)-\omega \left(t\right)\dot{y}\left(t\right)& \dot{s}\left(t\right)sin\theta \left(t\right)+\omega \left(t\right)\dot{x}\left(t\right)\end{array}\right]}^{\prime }.$$

(29)

Using a similar approach, the Itô SDE [45,48,49] for the Cartesian state is

$$d\mathbf{\eta }\left(t\right)={\mathbf{f}}_{\mathrm{c}}\left(\mathbf{\eta }\left(t\right)\right)dt+{\mathbf{G}}_{\mathrm{c}}\left(\mathbf{\eta }\left(t\right)\right)d\mathbf{\beta }\left(t\right),$$

(30)

where

$${\mathbf{f}}_{\mathrm{c}}\left(\mathbf{\eta }\left(t\right)\right):={\left[\begin{array}{ccccc}\dot{x}\left(t\right)& \dot{y}\left(t\right)& -\omega \left(t\right)\dot{y}\left(t\right)& \omega \left(t\right)\dot{x}\left(t\right)& 0\end{array}\right]}^{\prime },$$

(31)

$${\mathbf{G}}_{\mathrm{c}}\left(\mathbf{\eta }\left(t\right)\right):={\left[\begin{array}{ccccc}0& 0& \sqrt{q_s}\dot{x}\left(t\right)/s\left(t\right)& \sqrt{q_s}\dot{y}\left(t\right)/s\left(t\right)& 0\\ 0& 0& 0& 0& \sqrt{q_{\omega }}\end{array}\right]}^{\prime }.$$

(32)

3. Discretization of Target NCT Models

3.1. The Euler Approximation

The Euler approximation is obtained by applying the Itô lemma [48] to the integral form of the SDE and retaining only single integral terms. Applying the Euler approximation [50] to the 2D polar velocity dynamic model, we obtain the stochastic difference equation [45]:

$${\mathbf{\xi }}_k={\mathbf{\xi }}_{k-1}+T{\mathbf{f}}_{\mathrm{p}}\left({\mathbf{\xi }}_{k-1}\right)+\sqrt{T}{\mathbf{G}}_{\mathrm{p}}{\mathbf{w}}_1,$$

(33)

where ${\mathbf{f}}_{\mathrm{p}}\left(\mathbf{\xi }\right)$ is defined in (20) and

$${\mathbf{w}}_1\sim \mathcal{N}({\mathbf{w}}_1;{\mathbf{0}}_{2\times 1},{\mathbf{I}}_2).$$

(34)

The covariance of the polar velocity process noise ${\mathbf{w}}_{k-1}^{\mathbf{\xi }}=\sqrt{T}{\mathbf{G}}_{\mathrm{p}}{\mathbf{w}}_1$ is described by

$${\mathbf{w}}_{k-1}^{\mathbf{\xi }}\sim \mathcal{N}({\mathbf{w}}_{k-1}^{\mathbf{\xi }};{\mathbf{0}}_{5\times 1},{\mathbf{Q}}^{\mathbf{\xi }}),$$

(35)

$${\mathbf{Q}}^{\mathbf{\xi }}=\mathrm{diag}(0,0,q_{s}T,0,q_{\omega }T).$$

(36)

From (36), we see that the polar velocity process noise is independent of the state.

Similarly, applying the Euler approximation to the Cartesian velocity dynamic model, we obtain the stochastic difference equation [45]

$${\mathbf{\eta }}_k={\mathbf{\eta }}_{k-1}+T{\mathbf{f}}_{\mathrm{c}}\left({\mathbf{\eta }}_{k-1}\right)+\sqrt{T}{\mathbf{G}}_{\mathrm{c}}\left({\mathbf{\eta }}_{k-1}\right){\mathbf{w}}_1,$$

(37)

where ${\mathbf{f}}_{\mathrm{c}}\left(\mathbf{\eta }\right)$ is defined in (31). The Cartesian velocity process noise ${\mathbf{w}}_{k-1}^{\mathbf{\eta }}=\sqrt{T}{\mathbf{G}}_{\mathrm{c}}\left({\mathbf{\eta }}_{k-1}\right){\mathbf{w}}_1$ is described by

$${\mathbf{w}}_{k-1}^{\mathbf{\eta }}\left({\mathbf{\eta }}_{k-1}\right)\sim \mathcal{N}({\mathbf{w}}_{k-1}^{\mathbf{\eta }};{\mathbf{0}}_{5\times 1},{\mathbf{Q}}_{k-1}^{\mathbf{\eta }}),$$

(38)

$${\mathbf{Q}}_{k-1}^{\mathbf{\eta }}=TE\left\{{\mathbf{G}}_{\mathrm{c}}\left({\mathbf{\eta }}_{k-1}\right){\mathbf{G}}_{\mathrm{c}}^{\prime }\left({\mathbf{\eta }}_{k-1}\right)\right\}.$$

(39)

We make the following approximation in calculating $E\left\{{\mathbf{G}}_{\mathrm{c}}\left({\mathbf{\eta }}_{k-1}\right){\mathbf{G}}_{\mathrm{c}}^{\prime }\left({\mathbf{\eta }}_{k-1}\right)\right\}$,

$$E\left\{{\mathbf{G}}_{\mathrm{c}}\left({\mathbf{\eta }}_{k-1}\right){\mathbf{G}}_{\mathrm{c}}^{\prime }\left({\mathbf{\eta }}_{k-1}\right)\right\}\approx {\mathbf{G}}_{\mathrm{c}}({\widehat{\mathbf{\eta }}}_{k|k-1}){\mathbf{G}}_{\mathrm{c}}^{\prime }({\widehat{\mathbf{\eta }}}_{k|k-1}),$$

(40)

where ${\widehat{\mathbf{\eta }}}_{k|k-1}$ is the predicted Cartesian velocity state estimate at time k. Then,

$${\mathbf{Q}}_{k-1}^{\mathbf{\eta }}\approx T{\mathbf{G}}_{\mathrm{c}}({\widehat{\mathbf{\eta }}}_{k|k-1}){\mathbf{G}}_{\mathrm{c}}^{\prime }({\widehat{\mathbf{\eta }}}_{k|k-1}).$$

(41)

Simplification of (41) gives

$${\mathbf{Q}}_{k-1}^{\mathbf{\eta }}\approx {\left[\begin{array}{ccc}{\mathbf{0}}_2& {\mathbf{0}}_2& {\mathbf{0}}_{2\times 1}\\ {\mathbf{0}}_2& q_{s}T{\mathbf{A}}_{k-1}({\widehat{\mathbf{\eta }}}_{k|k-1})& {\mathbf{0}}_{2\times 1}\\ {\mathbf{0}}_{1\times 2}& {\mathbf{0}}_{1\times 2}& q_{\omega }T\end{array}\right]}^{\prime },$$

(42)

where

$${\mathbf{A}}_{k-1}({\widehat{\mathbf{\eta }}}_{k|k-1})=\frac{1}{{\widehat{s}}_{k|k-1}^2}{\left[\begin{array}{cc}{\widehat{\dot{x}}}_{k|k-1}^2& {\widehat{\dot{x}}}_{k|k-1}{\widehat{\dot{y}}}_{k|k-1}\\ {\widehat{\dot{x}}}_{k|k-1}{\widehat{\dot{y}}}_{k|k-1}& {\widehat{\dot{y}}}_{k|k-1}^2\end{array}\right]}^{\prime }.$$

(43)

From (42) and (43), we see that the Cartesian velocity-based process noise covariance is state-dependent.

From (4) and (5), we get the polar and Cartesian velocity-based states as

$${\mathbf{x}}_{\mathrm{p},k}={\left[\begin{array}{cc}{\mathbf{\xi }}_k^{\prime }& {\mathbf{z}}_k^{\prime }\end{array}\right]}^{\prime },$$

(44)

$${\mathbf{x}}_{\mathrm{c},k}={\left[\begin{array}{cc}{\mathbf{\eta }}_k^{\prime }& {\mathbf{z}}_k^{\prime }\end{array}\right]}^{\prime }.$$

(45)

The 3D discrete-time dynamic model for the polar velocity-based model is given by

$${\mathbf{x}}_{\mathrm{p},k}={\mathbf{x}}_{\mathrm{p},k-1}+T{\tilde{\mathbf{f}}}_{\mathrm{p}}\left({\mathbf{x}}_{\mathrm{p},k-1}\right)+{\mathbf{w}}_{\mathrm{p},k-1},$$

(46)

where ${\tilde{\mathbf{f}}}_{\mathrm{p}}\left({\mathbf{x}}_{\mathrm{p}}\right)$ is defined by

$${\tilde{\mathbf{f}}}_{\mathrm{p}}\left({\mathbf{x}}_{\mathrm{p}}\right)={\left[\begin{array}{ccccccc}scos\theta & ssin\theta & 0& \omega & 0& \dot{z}& 0\end{array}\right]}^{\prime },$$

(47)

$${\mathbf{w}}_{\mathrm{p},k-1}:={\left[\begin{array}{cc}{({\mathbf{w}}_{k-1}^{\mathbf{\xi }})}^{\prime }& {\mathbf{w}}_{\mathbf{z},k-1}^{\prime }\end{array}\right]}^{\prime },$$

(48)

$${\mathbf{w}}_{\mathrm{p},k-1}\sim \mathcal{N}({\mathbf{w}}_{\mathrm{p},k-1};{\mathbf{0}}_{7\times 1},{\mathbf{Q}}_{\mathrm{p}}),$$

(49)

$${\mathbf{Q}}_{\mathrm{p}}=\left[\begin{array}{cc}{\mathbf{Q}}^{\mathbf{\xi }}& {\mathbf{0}}_{5\times 2}\\ {\mathbf{0}}_{2\times 5}& {\mathbf{Q}}_{\mathbf{z}}\end{array}\right].$$

(50)

Similarly, the 3D discrete-time dynamic model for the Cartesian velocity-based model is given by

$${\mathbf{x}}_{\mathrm{c},k}={\mathbf{x}}_{\mathrm{c},k-1}+T{\tilde{\mathbf{f}}}_{\mathrm{c}}\left({\mathbf{x}}_{\mathrm{c},k-1}\right)+{\mathbf{w}}_{\mathrm{c},k-1},$$

(51)

where ${\tilde{\mathbf{f}}}_{\mathrm{c}}\left({\mathbf{x}}_{\mathrm{c}}\right)$ is defined by

$${\tilde{\mathbf{f}}}_{\mathrm{c}}\left({\mathbf{x}}_{\mathrm{c}}\right)={\left[\begin{array}{ccccccc}\dot{x}& \dot{y}& -\omega \dot{y}& \omega \dot{x}& 0& \dot{z}& 0\end{array}\right]}^{\prime },$$

(52)

$${\mathbf{w}}_{\mathrm{c},k-1}:={\left[\begin{array}{cc}{\left({\mathbf{w}}_{k-1}^{\mathbf{\eta }}\right)}^{\prime }& {\mathbf{w}}_{\mathbf{z},k-1}^{\prime }\end{array}\right]}^{\prime },$$

(53)

$${\mathbf{w}}_{\mathrm{c},k-1}\sim \mathcal{N}({\mathbf{w}}_{\mathrm{c},k-1};{\mathbf{0}}_{7\times 1},{\mathbf{Q}}_{\mathrm{c},k-1}),$$

(54)

$${\mathbf{Q}}_{\mathrm{c},k-1}=\left[\begin{array}{cc}{\mathbf{Q}}_{k-1}^{\mathbf{\eta }}& {\mathbf{0}}_{5\times 2}\\ {\mathbf{0}}_{2\times 5}& {\mathbf{Q}}_{\mathbf{z}}\end{array}\right].$$

(55)

3.2. Order 2 Weak Taylor Approximation

Using the order 2 weak Taylor approximation [50] to the SDE, we obtain the discretized dynamic model for the polar velocity-based model as [45]

$${\mathbf{\xi }}_k={\mathbf{\xi }}_{k-1}+T{\mathbf{f}}_{\mathrm{p},2}\left({\mathbf{\xi }}_{k-1}\right)+{\mathbf{G}}_{\mathrm{p},2}\left({\mathbf{\xi }}_{k-1}\right){\mathbf{w}}_2,$$

(56)

where

$${\mathbf{f}}_{\mathrm{p},2}\left({\mathbf{\xi }}_{k-1}\right)=\left[\begin{array}{c}{s}_{k-1}cos\left({\theta }_{k-1}\right)-T{s}_{k-1}{\omega }_{k-1}sin\left({\theta }_{k-1}\right)/2\\ s_{k-1}sin\left({\theta }_{k-1}\right)+T{s}_{k-1}{\omega }_{k-1}cos\left({\theta }_{k-1}\right)/2\\ 0\\ {\omega }_{k-1}\\ 0\end{array}\right],$$

(57)

$${\mathbf{G}}_{\mathrm{p},2}\left({\mathbf{\xi }}_{k-1}\right)={\mathbf{E}}_{\mathrm{p}}\left({\mathbf{\xi }}_{k-1}\right)\mathbf{V}\left(T\right),$$

(58)

$${\mathbf{E}}_{\mathrm{p}}\left({\mathbf{\xi }}_{k-1}\right)=\left[\begin{array}{cccc}\sqrt{q_s}cos\left({\theta }_{k-1}\right)& 0& 0& 0\\ \sqrt{q_s}sin\left({\theta }_{k-1}\right)& 0& 0& 0\\ 0& 0& \sqrt{q_s}& 0\\ 0& \sqrt{q_{\omega }}& 0& 0\\ 0& 0& 0& \sqrt{q_{\omega }}\end{array}\right],$$

(59)

$$\mathbf{V}\left(T\right)=\left[\begin{array}{cc}\sqrt{T^3/3}& 0\\ \sqrt{3T}/2& \sqrt{T}/2\end{array}\right]\otimes {\mathbf{I}}_2,$$

(60)

$${\mathbf{w}}_2\sim \mathcal{N}({\mathbf{w}}_2;{\mathbf{0}}_{4\times 1},{\mathbf{I}}_4).$$

(61)

In (60), ⊗ refers to the Kronecker product [52].

The process noise ${\mathbf{w}}_{\mathrm{p},2,k-1}={\mathbf{G}}_{\mathrm{p},2}\left({\mathbf{\xi }}_{k-1}\right){\mathbf{w}}_2$ and associated covariance ${\mathbf{Q}}_{\mathrm{p},2,k-1}$ for the second-order polar velocity-based model are described, respectively, by

$${\mathbf{w}}_{\mathrm{p},2,k-1}\sim \mathcal{N}({\mathbf{w}}_{\mathrm{p},2,k-1};{\mathbf{0}}_{5\times 1},{\mathbf{Q}}_{\mathrm{p},2,k-1}),$$

(62)

$${\mathbf{Q}}_{\mathrm{p},2,k-1}=E\{{\mathbf{G}}_{\mathrm{p},2}\left({\mathbf{\xi }}_{k-1}\right){\mathbf{G}}_{\mathrm{p},2}^{\prime }\left({\mathbf{\xi }}_{k-1}\right)\}.$$

(63)

Using a similar approximation as before, we obtain

$${\mathbf{Q}}_{\mathrm{p},2,k-1}\approx {\mathbf{G}}_{\mathrm{p},2}({\widehat{\mathbf{\xi }}}_{k|k-1}){\mathbf{G}}_{\mathrm{p},2}^{\prime }({\widehat{\mathbf{\xi }}}_{k|k-1}),$$

(64)

where ${\widehat{\mathbf{\xi }}}_{k|k-1}$ is the predicted polar velocity state estimate at time $k.$

Simplification of ${\mathbf{G}}_{\mathrm{p},2}\left({\mathbf{\xi }}_{k-1}\right){\mathbf{G}}_{\mathrm{p},2}^{\prime }\left({\mathbf{\xi }}_{k-1}\right)$ gives

$${\mathbf{G}}_{\mathrm{p},2}\left({\mathbf{\xi }}_{k-1}\right){\mathbf{G}}_{\mathrm{p},2}^{\prime }\left({\mathbf{\xi }}_{k-1}\right)=\left[\begin{array}{ccccc}\frac{T^3{cos}^2\left({\theta }_{k-1}\right)}{3}{q}_s& \frac{T^{3}sin\left(2{\theta }_{k-1}\right)}{6}{q}_s& \frac{T^{2}cos\left({\theta }_{k-1}\right)}{2}{q}_s& 0& 0\\ \frac{T^{3}sin\left(2{\theta }_{k-1}\right)}{6}{q}_s& \frac{T^3{sin}^2\left({\theta }_{k-1}\right)}{3}{q}_s& \frac{T^{2}sin\left({\theta }_{k-1}\right)}{2}{q}_s& 0& 0\\ \frac{T^{2}cos\left({\theta }_{k-1}\right)}{2}{q}_s& \frac{T^{2}sin\left({\theta }_{k-1}\right)}{2}{q}_s& T{q}_s& 0& 0\\ 0& 0& 0& \frac{T^3}{3}{q}_{\omega }& \frac{T^2}{2}{q}_{\omega }\\ 0& 0& 0& \frac{T^2}{2}{q}_{\omega }& T{q}_{\omega }\end{array}\right].$$

(65)

The discretized dynamic model for the Cartesian velocity-based model using the TS2 approximation to the SDE is given by [45]

$${\mathbf{\eta }}_k={\mathbf{\eta }}_{k-1}+T{\mathbf{f}}_{\mathrm{c},2}\left({\mathbf{\eta }}_{k-1}\right)+{\mathbf{G}}_{\mathrm{c},2}\left({\mathbf{\eta }}_{k-1}\right){\mathbf{w}}_2,$$

(66)

where

$${\mathbf{f}}_{\mathrm{c},2}\left({\mathbf{\eta }}_{k-1}\right)=\left[\begin{array}{c}{\dot{x}}_{k-1}-T{\omega }_{k-1}{\dot{y}}_{k-1}/2\\ {\dot{y}}_{k-1}+T{\omega }_{k-1}{\dot{x}}_{k-1}/2\\ -{\omega }_{k-1}{\dot{y}}_{k-1}-T{\omega }_{k-1}^2{\dot{x}}_{k-1}/2\\ {\omega }_{k-1}{\dot{x}}_{k-1}-T{\omega }_{k-1}^2{\dot{y}}_{k-1}/2\\ 0\end{array}\right],$$

(67)

$${\mathbf{G}}_{\mathrm{c},2}\left({\mathbf{\eta }}_{k-1}\right)={\mathbf{E}}_{\mathrm{c}}\left({\mathbf{\eta }}_{k-1}\right)\mathbf{V}\left(T\right),$$

(68)

$${\mathbf{E}}_{\mathrm{c}}\left({\mathbf{\eta }}_{k-1}\right)=\left[\begin{array}{cccc}\sqrt{q_s}{\dot{x}}_{k-1}/s_{k-1}& 0& 0& 0\\ \sqrt{q_s}{\dot{y}}_{k-1}/s_{k-1}& 0& 0& 0\\ 0& -\sqrt{q_{\omega }}{\dot{y}}_{k-1}& \sqrt{q_s}({\dot{x}}_{k-1}-T{\omega }_{k-1}{\dot{y}}_{k-1})/s_{k-1}& 0\\ 0& \sqrt{q_{\omega }}{\dot{x}}_{k-1}& \sqrt{q_s}({\dot{y}}_{k-1}+T{\omega }_{k-1}{\dot{x}}_{k-1})/s_{k-1}& 0\\ 0& 0& 0& \sqrt{q_{\omega }}\end{array}\right].$$

(69)

The process noise ${\mathbf{w}}_{\mathrm{c},2,k-1}={\mathbf{G}}_{\mathrm{c},2}\left({\mathbf{\eta }}_{k-1}\right){\mathbf{w}}_2$, and corresponding covariance ${\mathbf{Q}}_{\mathrm{c},2,k-1}$ for the second-order Cartesian velocity-based model are given, respectively, by

$${\mathbf{w}}_{\mathrm{c},2,k-1}\sim \mathcal{N}({\mathbf{w}}_{\mathrm{c},2,k-1};{\mathbf{0}}_{5\times 1},{\mathbf{Q}}_{\mathrm{c},2,k-1}),$$

(70)

$${\mathbf{Q}}_{\mathrm{c},2,k-1}=E\{{\mathbf{G}}_{\mathrm{c},2}\left({\mathbf{\eta }}_{k-1}\right){\mathbf{G}}_{\mathrm{c},2}^{\prime }\left({\mathbf{\eta }}_{k-1}\right)\}.$$

(71)

The approximate expression for the process noise is given by

$${\mathbf{Q}}_{\mathrm{c},2,k-1}\approx {\mathbf{G}}_{\mathrm{c},2}({\widehat{\mathbf{\eta }}}_{k|k-1}){\mathbf{G}}_{\mathrm{c},2}^{\prime }({\widehat{\mathbf{\eta }}}_{k|k-1}),$$

(72)

where ${\widehat{\mathbf{\eta }}}_{k|k-1}$ is the predicted state estimate at time $k.$ Simplification of ${\mathbf{G}}_{\mathrm{c},2}\left({\mathbf{\eta }}_{k-1}\right){\mathbf{G}}_{\mathrm{c},2}^{\prime }\left({\mathbf{\eta }}_{k-1}\right)$ gives

$$\begin{array}{cc}\hfill & {\mathbf{G}}_{\mathrm{c},2}\left({\mathbf{\eta }}_{k-1}\right){\mathbf{G}}_{\mathrm{c},2}^{\prime }\left({\mathbf{\eta }}_{k-1}\right)=\hfill \\ \hfill & \left[\begin{array}{ccccc}\frac{T^3}{3}{a}_1^2{q}_s& \frac{T^3}{3}{a}_1{a}_2{q}_s& \frac{T^2}{2}{a}_1{a}_3{q}_s& \frac{T^2}{2}{a}_1{a}_4{q}_s& 0\\ \frac{T^3}{3}{a}_1{a}_2{q}_s& \frac{T^3}{3}{a}_2^2{q}_s& \frac{T^2}{2}{a}_2{a}_3{q}_s& \frac{T^2}{2}{a}_2{a}_4{q}_s& 0\\ \frac{T^2}{2}{a}_1{a}_3{q}_s& \frac{T^2}{2}{a}_2{a}_3{q}_s& T{a}_3^2{q}_s+\frac{T^3}{3}{\dot{y}}_{k-1}^2{q}_{\omega }& T{a}_3{a}_4{q}_s-\frac{T^3}{3}{\dot{x}}_{k-1}{\dot{y}}_{k-1}{q}_{\omega }& -\frac{T^2}{2}{\dot{y}}_{k-1}{q}_{\omega }\\ \frac{T^2}{2}{a}_1{a}_4{q}_s& \frac{T^2}{2}{a}_2{a}_4{q}_s& T{a}_3{a}_4{q}_s-\frac{T^3}{3}{\dot{x}}_{k-1}{\dot{y}}_{k-1}{q}_{\omega }& T{a}_4^2{q}_s+\frac{T^3}{3}{\dot{x}}_{k-1}^2{q}_{\omega }& \frac{T^2}{2}{\dot{x}}_{k-1}{q}_{\omega }\\ 0& 0& -\frac{T^2}{2}{\dot{y}}_{k-1}{q}_{\omega }& \frac{T^2}{2}{\dot{x}}_{k-1}{q}_{\omega }& T{q}_{\omega }\end{array}\right],\hfill \end{array}$$

(73)

where

$$a_1=\frac{{\dot{x}}_{k-1}}{s_{k-1}},a_2=\frac{{\dot{y}}_{k-1}}{s_{k-1}},$$

(74)

$$a_3=\frac{{\dot{x}}_{k-1}-T{\omega }_{k-1}{\dot{y}}_{k-1}}{s_{k-1}},a_4=\frac{{\dot{y}}_{k-1}+T{\omega }_{k-1}{\dot{x}}_{k-1}}{s_{k-1}}.$$

(75)

3.3. Comparison with Conventional NCT Model

We consider the NCT model using the Cartesian velocity-based state, when the angular rate is estimated. The NCT model using the direct discrete approach is described in Chapter 11 of [22]. The discretized continuous-time NCT model [27] is described by

$${\mathbf{\eta }}_k={\mathbf{F}}_{\mathrm{NCT}}^{\mathrm{C}}\left(\omega \right){\mathbf{\eta }}_{k-1}+{\mathbf{w}}_{k-1}^{\mathrm{C}},$$

(76)

$${\mathbf{F}}_{\mathrm{NCT}}^{\mathrm{C}}\left(\omega \right)=\left[\begin{array}{ccccc}1& 0& \frac{sin\left(\omega T\right)}{\omega }& -\frac{1-cos\left(\omega T\right)}{\omega }& 0\\ 0& 1& \frac{1-cos\left(\omega T\right)}{\omega }& \frac{sin\left(\omega T\right)}{\omega }& 0\\ 0& 0& cos\left(\omega T\right)& -sin\left(\omega T\right)& 0\\ 0& 0& sin\left(\omega T\right)& cos\left(\omega T\right)& 0\\ 0& 0& 0& 0& 1\end{array}\right],$$

(77)

$${\mathbf{w}}_{k-1}^{\mathrm{C}}\sim \mathcal{N}({\mathbf{w}}_{k-1}^{\mathrm{C}};\mathbf{0},{\mathbf{Q}}_{k-1}^{\mathrm{C}}),$$

(78)

$${\mathbf{Q}}_{k-1}^{\mathrm{C}}=\left[\begin{array}{ccccc}q{T}^3/3& 0& q{T}^2/2& 0& 0\\ 0& q{T}^3/3& 0& q{T}^2/2& 0\\ q{T}^2/2& 0& qT& 0& 0\\ 0& q{T}^2/2& 0& qT& 0\\ 0& 0& 0& 0& q_{\omega }T\end{array}\right],$$

(79)

where q is the PSD of the acceleration process noise along the X or Y direction.

Remark 1.

The upper left $4\times 4$ block of the state transition matrix in (77) is the state transition matrix for the NCV model using Cartesian state [22]. Similarly, the upper left $4\times 4$ block of the process noise covariance matrix in (79) is the process noise covariance matrix for the NCV model using Cartesian state [22]. They cannot be derived from a continuous-time model of the NCT motion.

The second-order model with the TS2 approximation and Cartesian velocity-based state was used in [53], and a superior RMSE was reported compared with the conventional model described above.

4. Sensor Dynamic and Measurement Models

4.1. Sensor Dynamic Models

We assume that the motion of the sensor is deterministic and the state of the sensor at each measurement time is exactly known. The sensor follows two types of motion: constant velocity (CV) in 3D and the second type of motion with known angular velocity $\mathsf{\Omega }.$ For both types of motion, the Cartesian state vector of the sensor is appropriate and is defined by

$${\mathbf{x}}^{\mathrm{s}}\left(t\right):={\left[\begin{array}{cccccc}{x}^{\mathrm{s}}\left(t\right)& y^{\mathrm{s}}\left(t\right)& {\dot{x}}^{\mathrm{s}}\left(t\right)& {\dot{y}}^{\mathrm{s}}\left(t\right)& z^{\mathrm{s}}\left(t\right)& {\dot{z}}^{\mathrm{s}}\left(t\right)\end{array}\right]}^{\prime }.$$

(80)

The dynamic models of the sensor for the CV and CT are described, respectively, by [8,31]

$${\mathbf{x}}_k^{\mathrm{s}}={\mathbf{F}}^{\mathrm{CV}}\left(T\right){\mathbf{x}}_{k-1}^{\mathrm{s}},$$

(81)

$${\mathbf{F}}^{\mathrm{CV}}\left(T\right)=\left[\begin{array}{ccc}{\mathbf{I}}_2& {\mathbf{I}}_{2}T& {\mathbf{0}}_2\\ {\mathbf{0}}_2& {\mathbf{I}}_2& {\mathbf{0}}_2\\ {\mathbf{0}}_2& {\mathbf{0}}_2& {\mathbf{F}}_1\end{array}\right].$$

(82)

$${\mathbf{x}}_k^{\mathrm{s}}={\mathbf{F}}^{\mathrm{CT}}(T,{\mathsf{\Omega }}_{k-1}){\mathbf{x}}_{k-1}^{\mathrm{s}},$$

(83)

where the state transition matrix ${\mathbf{F}}_1$ for CV is defined in (7), ${\mathsf{\Omega }}_{k-1}$ is the angular velocity of the sensor during $[t_{k-1},t_k)$ and the state transition matrix for CT is given by

$${\mathbf{F}}^{\mathrm{CT}}(T,\mathsf{\Omega })=\left[\begin{array}{cccccc}1& 0& sin\left(\mathsf{\Omega }T\right)/\mathsf{\Omega }& -[1-cos(\mathsf{\Omega }T\left)\right]/\mathsf{\Omega }& 0& 0\\ 0& 1& [1-cos(\mathsf{\Omega }T\left)\right]/\mathsf{\Omega }& sin\left(\mathsf{\Omega }T\right)/\mathsf{\Omega }& 0& 0\\ 0& 0& cos\left(\mathsf{\Omega }T\right)& -sin\left(\mathsf{\Omega }T\right)& 0& 0\\ 0& 0& sin\left(\mathsf{\Omega }T\right)& cos\left(\mathsf{\Omega }T\right)& 0& 0\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right].$$

(84)

In passive IRST tracking, the sensor moves with a sequence of CV and CT motions [8,31].

4.2. Measurement Model

Let ${\mathbf{p}}_k$ and ${\mathbf{p}}_k^{\mathrm{s}}$ denote the target and sensor position vectors, respectively, at time $t_k$,

$${\mathbf{p}}_k:={\left[x_k{y}_k{z}_k\right]}^{\prime },$$

(85)

$${\mathbf{p}}_k^{\mathrm{s}}:={\left[x_k^{\mathrm{s}}{y}_k^{\mathrm{s}}{z}_k^{\mathrm{s}}\right]}^{\prime }.$$

(86)

An IRST sensor measures the bearing and elevation angles of a target [5,8], as shown in Figure 2. We note that the bearing (${\varphi }_k$) and elevation (${ϵ}_k$) angles depend on the relative position ${\mathbf{p}}_k-{\mathbf{p}}_k^{\mathrm{s}}$ in Cartesian and polar velocity-based models. Hence, for both type of state vectors, the measurement model for the bearing and elevation angles is described by

$${\mathbf{y}}_k=\mathbf{h}({\mathbf{p}}_k,{\mathbf{p}}_k^{\mathrm{s}})+{\mathbf{n}}_k,$$

(87)

$$\mathbf{h}({\mathbf{p}}_k,{\mathbf{p}}_k^{\mathrm{s}}):=\left[\begin{array}{c}{\varphi }_k\\ {ϵ}_k\end{array}\right]=\left[\begin{array}{c}{tan}^{-1}(x_k-x_k^{\mathrm{s}},y_k-y_k^{\mathrm{s}})\\ arctan((z_k-z_k^{\mathrm{s}})/{\rho }_k)\end{array}\right],$$

(88)

where ${\varphi }_k$ and ${ϵ}_k$ lie in $[0,2\pi )$ and $(-\pi /2,\pi /2)$, respectively and the ground range ${\rho }_k$ is defined by

$${\rho }_k:=\sqrt{{(x_k-x_k^{\mathrm{s}})}^2+{(y_k-y_k^{\mathrm{s}})}^2},{\rho }_k>0.$$

(89)

We assume that the measurement noise is zero-mean Gaussian with covariance $\mathbf{R}$

$${\mathbf{n}}_k\sim \mathcal{N}({\mathbf{n}}_k;\mathbf{0},\mathbf{R}),$$

(90)

$$\mathbf{R}:=\mathrm{diag}({\sigma }_{\varphi }^2,{\sigma }_{ϵ}^2),$$

(91)

where ${\sigma }_{\varphi }$ and ${\sigma }_{ϵ}$ are the bearing and elevation measurement standard deviations (SDs), respectively.

5. Filtering Algorithms

We compare the performances of four CKF-based algorithms using the Euler and TS2 approximations with the polar and Cartesian velocity-based states. These four algorithms are called CKF1P, CKF1C, CKF2P, and CKF1P. The discrete-time dynamic and measurement models in these algorithms are nonlinear. The features of these algorithms are summarized in

Table 1. Features of CKF based algorithms.

Filter	2D State in NCT	Approximation	Process Noise
CKF1P	Polar velocity	Euler	State-independent
CKF1C	Cartesian velocity	Euler	State-dependent
CKF2P	Polar velocity	TS2	State-dependent
CKF2C	Cartesian velocity	TS2	State-dependent

. In [54], the authors have considered the maneuvering target tracking problem using a CKF-based CDF filter with range, azimuth, and elevation measurements. They claim that this is a very challenging problem. They use the prior distribution to initialize the filter. The problem considered in our study is relatively harder since only azimuth, and elevation measurements are available.

6. Numerical Simulation and Results

The IRST sensor trajectory and parameters in the simulation are similar to those used in [8,31]. The target moves with an NCT motion in a plane parallel to the $XY$-plane and moves with an NCV motion along the Z-axis.

Table 2. Prior polar mean velocity-based 3D state parameters of target.

Variable	Value
${\overline{x}}_0$ (m)	97,580.7358
${\overline{y}}_0$ (m)	97,580.7358
${\overline{s}}_0$ (m/s)	297.0
${\overline{\theta }}_0$ (deg)	215.0
${\overline{\omega }}_0$ (deg/s)	5.672
${\overline{z}}_0$ (m)	9000.0
${\overline{\dot{z}}}_0$ (m/s)	0.0

presents prior mean polar velocity-based state parameters of the target. The NCT motion has a centripetal acceleration $s_1{\omega }_1$ of 3 g, where $g=9.8{\mathrm{m}}^2{\mathrm{s}}^{-2}$. This scenario was used in [5]. We use the same filter initialization with that in [54] in the current study. The prior variance of the 3D polar velocity-based state is chosen as

$$\begin{array}{cc}\hfill {\mathbf{P}}_{0,\mathrm{p},1}& =\mathrm{diag}({1000}^2 {\mathrm{m}}^2,{1000}^2 {\mathrm{m}}^2,{30}^2 {\mathrm{m}}^2{\mathrm{s}}^{-2},0.0873 {\mathrm{rad}}^2,{(4.95\times {10}^{-3})}^2 {\mathrm{rad}}^2{\mathrm{s}}^{-2},\hfill \\ \hfill & {100}^2 {\mathrm{m}}^2,5^2 {\mathrm{m}}^2{\mathrm{s}}^{-2}).\hfill \end{array}$$

(92)

Using the Euler approximation, the process noise covariance in the polar velocity-based model for the NCT motion can be calculated exactly. Hence, we use the Euler approximation for the polar velocity-based model to generate true target trajectories for the NCT motion in the $XY$-plane using 100 sub-sampling intervals for the measurement time interval (T) of 1 s. The Z-component of the NCV trajectory is generated exactly. The process noise parameters used in the simulation are $q_s=0.2{\mathrm{m}}^2{\mathrm{s}}^{-3}$, $q_{\omega }=5e-07{\mathrm{rad}}^2{\mathrm{s}}^{-3}$, and $q_z=0.001{\mathrm{m}}^2{\mathrm{s}}^{-3}.$ Figure 3 presents the true NCT trajectory of the target in the $XY$-plane from the first Monte Carlo run.

We assume that the motion of the sensor is deterministic. The sensor moves in a plane parallel to the $XY$-plane at a fixed height of 10 km and follows a sequence of CV and CT motions. The initial position and velocity vectors of the sensor are (0, 0, 10,000) m and (0, 264, 0) m/s, respectively.

Table 3. Motion profile of the sensor.

Interval (s)	$\Delta \mathit{t}$ (s)	$\Delta {\mathit{\varphi }}_{\mathbf{s}}$ (rad)	Motion Type	$\mathsf{\Omega }$ (rad/s)
$[0,15]$	15	0	CV	0
$[15,31]$	16	$-\pi /4$	CT	$-\pi /64$
$[31,43]$	12	0	CV	0
$[43,75]$	32	$\pi /2$	CT	$\pi /64$
$[75,86]$	11	0	CV	0
$[86,102]$	16	$-\pi /4$	CT	$-\pi /64$

presents the motion profile of the sensor. In Table 3, $\Delta t$ represents the duration of the segment, $\Delta {\varphi }_{\mathrm{s}}$ is the total angular change during the segment, and $\mathsf{\Omega }$ is the angular velocity of the sensor during the segment. The measurement time interval of the IRST sensor is 1 s and there are 101 measurements. The measurement error SDs for bearing and elevation have the same value. We use two angle SDs of 1 mrad and 2 mrad in this simulation. The sensor trajectory in the $XY$-plane is shown in Figure 4.

6.1. Comparison of Filtering Algorithms

We used 500 Monte Carlo runs to compute the root mean square (RMS) position, velocity, and angular rate errors of the CKF1P, CKF1C, CKF2P, and CKF2C. Each filter is initialized using the prior mean and covariance. The RMS errors for these four filters for angle SDs of 1 mrad and 2 mrad are presented in Figure 5, Figure 6 and Figure 7. Results in Figure 5 show that RMS position errors of the CKF1P, CKF2P, and CKF2C are close and they are nearly the same towards the end. On the contrary, the RMS position error of the CKF1C is significantly higher during some measurement intervals and also significantly lower during a time interval. We see in Figure 6 that the CKF2P and CKF2C have the best results and nearly the same RMS velocity errors. The RMS velocity error of the CKF1P is slightly higher than that CKF2P and CKF2C. The RMS velocity error of the CKF1C is significantly higher than that of the other three filters. It appears that the CKF1C diverges for this maneuvering target tracking scenario. A similar pattern is observed in the results of the angular rate errors in Figure 7.

To evaluate the overall performance of a filter, we use the root time-averaged mean square (RTAMS) error [11] for position, velocity, and angular rate. Let ${\mathbf{p}}_{j,i}^{\mathrm{t}}$ and ${\widehat{\mathbf{p}}}_{j,i}^{\mathrm{t}}$ denote the true and estimated position of the target, respectively, at time index j in the ith Monte Carlo run. The RTAMS position error [11] is defined by

$${\mathrm{RTAMS}}_{\mathrm{pos}}=\sqrt{\frac{1}{N_{\mathrm{f}}M}\sum _{j\in S_{\mathrm{f}}}\sum _{i=1}^M{\parallel {\widehat{\mathbf{p}}}_{j,i}^{\mathrm{t}}-{\mathbf{p}}_{k,j}^{\mathrm{t}}\parallel }^2},$$

(93)

where $S_{\mathrm{f}}$ is a set of time indices with $N_{\mathrm{f}}$ indices, and M is the number of Monte Carlo runs. We have chosen time indices from 51 to 101 to define $S_{\mathrm{f}}$. The RTAMS error [11] for velocity and angular rate are similarly defined.

Table 4. RTAMS position, velocity, and angular rate errors for CKF1P, CKF1C, CKF2P, and CKF2C using the prior variance ${\mathbf{P}}_{0,\mathrm{p},1}$.

Metric	Filter	1 mrad	2 mrad
Position error (km)	CKF1P	1.137	1.355
	CKF1C	1.596	1.582
	CKF2P	1.165	1.400
	CKF2C	1.146	1.389
Velocity error (m/s)	CKF1P	28.628	28.178
	CKF1C	75.853	77.691
	CKF2P	18.959	21.132
	CKF2C	18.867	21.334
Angular rate error (deg/s)	CKF1P	0.197	0.211
	CKF1C	0.394	0.347
	CKF2P	0.197	0.214
	CKF2C	0.197	0.216

presents the RTAMS error metric for position, velocity, and angular rate for measurement error SDs of 1 mrad and 2 mrad. Results in Table 4 show that the CKF2P and CKF2C have the best RTAMS errors for position, velocity, and angular rate, which are nearly the same.

Table 5. CPU times (s) for each Monte Carlo run and CPU times relative to CKF1P for angle SD of 1 mrad.

Filter	CPU Time (s)	CPU Relative to CKF1P
CKF1P	0.0377	1.0000
CKF1C	0.0386	1.0129
CKF2P	0.0391	1.0356
CKF2C	0.0789	2.0910

presents CPU times for each Monte Carlo run and CPU times relative to the CKF1P. Results in Table 5 show that the CKF1P has the fastest CPU time, being slightly faster than the CKF2P.

Let $x_{k,i}$ and ${\widehat{x}}_{k,i}$ be the true and filtered estimated X-coordinates at time $k,$ respectively. Similar definitions apply for other position coordinates. Then, the sample position bias is given by [22,33]

$$b_{\mathrm{pos},k}=\frac{1}{M}\sum _{i=1}^M[(x_{k,i}-{\widehat{x}}_{k,i})+(y_{k,i}-{\widehat{y}}_{k,i})+(z_{k,i}-{\widehat{z}}_{k,i})].$$

(94)

For simplicity, we use “bias” to represent sample bias. Similarly, the biases for velocity and angular rate can be defined. The bias at time k can be positive or negative. It is desirable to have a small bias in the state estimate. The sample bias for position, velocity, and angular rate are shown in Figure 8 and Figure 9. Results in Figure 8 show that the position biases of the CKF1P, CKF2P, and CKF2C are small, and the velocity biases of CKF2P and CKF2C are nearly zero. The angular rate biases of CKF1P, CKF2P, and CKF2C become smaller with time and approach zero. The CKF1C has large position, velocity, and angular rate biases.

6.2. Dependence of Filtering Accuracy on the Prior Distribution

In order to analyze the dependence of filtering accuracy on the prior distribution, we have chosen a larger prior variance for the 3D polar velocity-based state relative to that used in (92)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{0,\mathrm{p},2}& =\mathrm{diag}({5000}^2 {\mathrm{m}}^2,{5000}^2 {\mathrm{m}}^2,{90}^2 {\mathrm{m}}^2{\mathrm{s}}^{-2},(3\ast 0.0873) {\mathrm{rad}}^2,{(3\ast 4.95\times {10}^{-3})}^2 {\mathrm{rad}}^2{\mathrm{s}}^{-2},\hfill \\ \hfill & {500}^2 {\mathrm{m}}^2,{15}^2 {\mathrm{m}}^2{\mathrm{s}}^{-2}).\hfill \end{array}$$

(95)

The prior variance of Cartesian position is increased by 25 times, and the other components have been increased by 9 times. The prior mean is unchanged. The RMSE plots of position, velocity, and angular rate are presented in Figure 10 and Figure 11 for the 1 mrad scenario.

Table 6. Comparison of RTAMS position, velocity, and angular rate errors for CKF1P, CKF1C, CKF2P, CKF2C using prior variances ${\mathbf{P}}_{0,\mathbf{p},1}$ and ${\mathbf{P}}_{0,\mathrm{p},2}$ with angle SD of 1 mrad.

Metric	Filter	${\mathbf{P}}_{0,p,1}$	${\mathbf{P}}_{0,p,2}$
Position error (km)	CKF1P	1.137	7.175
	CKF1C	1.596	13.559
	CKF2P	1.165	7.178
	CKF2C	1.146	7.454
Velocity error (m/s)	CKF1P	28.628	47.204
	CKF1C	75.853	75.192
	CKF2P	18.959	43.836
	CKF2C	18.867	42.973
Angular rate error (deg/s)	CKF1P	0.197	0.265
	CKF1C	0.394	0.440
	CKF2P	0.197	0.265
	CKF2C	0.197	0.274

presents the RTAMS error for position, velocity, and angular rate for measurement error SDs of 1 mrad and the second prior distribution. Results in Table 6 show that the CKF2P and CKF2C have the best RTAMS errors for position, velocity, and angular rate, which are nearly the same.

6.3. Summary of Key Results

Based on RMS and RTAMS errors, the key results of our study are as follows:

• The CKF1P has the best position estimation accuracy. The position estimation accuracies of the CKF2P and CKF2C are close to that of the CKF1P;
• The CKF2P and CKF2C have the best velocity estimation accuracy;
• The state estimation accuracies of the CKF2P and CKF2C are comparable. However, the computational cost of the CKF2C is about twice that of the CKF2P;
• The CKF1C does not perform well for this problem and has high estimation errors.

7. Conclusions

We considered the challenging filtering problem of a maneuvering target in 3D using the bearing and elevation measurements from a maneuvering passive IRST sensor. Research on this problem is rather limited. The target moves with the NCT motion in the $XY$-plane and has an NCV motion along the Z-axis. We discretized the continuous-time stochastic differential equation for the NCT model using the first (Euler) and second-order Taylor approximations to obtain discrete-time NCT models. Discrete-time dynamic and measurement models are nonlinear. For each approximation, we used the polar and Cartesian velocity-based states for the NCT model. The CKF was used in each case giving rise to four filters: CKF1P, CKF1C, CKF2P, and CKF2C. Numerical results based on Monte Carlo simulations suggest that the second-order Taylor approximation-based filters CKF2P and CKF2C have the best state estimation accuracy for this scenario. Secondly, the accuracies of these two filters are nearly the same.

Our future work will develop filter initialization algorithms that can be used with real data. We shall also focus on calculating the PCRLB for the filtering problem to assess the best achievable accuracy.