Angle-Only Filtering of a Maneuvering Target in 3D

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.


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, 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 nonmaneuvering 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 A denotes the transpose of the vector or matrix A. The n−dimensional identity matrix, m−dimensional null matrix, and m × n null matrix are denoted by I n , 0 m , and 0 m×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.

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 ω is also estimated in the NCT model.
Let z(t) denote the Cartesian state along the Z-axis with position and velocity components For the NCT model in the XY-plane, we use η(t) and ξ(t) for state vectors where the angular velocity ω is estimated. The velocity in η(t) and ξ(t) has Cartesian and polar coordinates, respectively. Let s(t) and θ(t) 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 η(t) and ξ(t) are defined, respectively, by Three-dimensional state vectors where angular velocity is estimated are defined, respectively, by 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 where F 1 is the state transition matrix and w z,k−1 is a zero-mean white Gaussian process noise with covariance Q z , 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 ξ(t) is given bẏ We Sinceθ(t) = ω(t), (10) can be written aṡ The time derivative of η(t) iṡ 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ṡ where w s (t) and w ω (t) are continuous-time zero-mean white acceleration and angular acceleration process noises with power spectral densities q s and q ω , respectively, [22] E{w E{w where δ is the Dirac delta function [51]. We can write (14)-(16) mathematically rigorously by defining where dβ s (t) and dβ ω (t) are standard independent Wiener processes [45,48] E{dβ Define Then, conventionally, we can writeξ(t) as [23,48] We can write the time derivative of the polar state vector mathematically rigorously using the Itô stochastic differential equation (SDE) [45,48,49] where We assume that the prior distribution of ξ is Gaussian, The time derivative of η contains Cartesian accelerationsẍ andÿ in (13). It is necessary to transform them to derivatives of speed and angular velocity. The 2D Cartesian velocity is given by v(t) = ẋ(t)ẏ(t) (28) and the Cartesian acceleration isv(t). Using (11), the Cartesian acceleration is expressed bẏ Using a similar approach, the Itô SDE [45,48,49] for the Cartesian state is where

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]: where f p (ξ) is defined in (20) and The covariance of the polar velocity process noise w 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] where f c (η) is defined in (31). The Cartesian velocity process noise w We make the following approximation in calculating E{G c (η k−1 )G c (η k−1 )}, whereη k|k−1 is the predicted Cartesian velocity state estimate at time k. Then, Simplification of (41) gives where 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 The 3D discrete-time dynamic model for the polar velocity-based model is given by Similarly, the 3D discrete-time dynamic model for the Cartesian velocity-based model is given by

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] where In (60), ⊗ refers to the Kronecker product [52]. The process noise w p,2,k−1 = G p,2 (ξ k−1 )w 2 and associated covariance Q p,2,k−1 for the second-order polar velocity-based model are described, respectively, by Using a similar approximation as before, we obtain whereξ k|k−1 is the predicted polar velocity state estimate at time k. Simplification of G p,2 (ξ k−1 )G p,2 (ξ k−1 ) gives The discretized dynamic model for the Cartesian velocity-based model using the TS2 approximation to the SDE is given by [45] where The process noise w c,2,k−1 = G c,2 (η k−1 )w 2 , and corresponding covariance Q c,2,k−1 for the second-order Cartesian velocity-based model are given, respectively, by The approximate expression for the process noise is given by whereη k|k−1 is the predicted state estimate at time k. Simplification of G c,2 (η k−1 )G c,2 (η k−1 ) gives where

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 where q is the PSD of the acceleration process noise along the X or Y direction. (77) is the state transition matrix for the NCV model using Cartesian state [22]. Similarly, the upper left 4 × 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.

Remark 1. The upper left 4 × 4 block of the state transition matrix in
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.

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 Ω. For both types of motion, the Cartesian state vector of the sensor is appropriate and is defined by The dynamic models of the sensor for the CV and CT are described, respectively, by [8,31] x s where the state transition matrix F 1 for CV is defined in (7), Ω 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 In passive IRST tracking, the sensor moves with a sequence of CV and CT motions [8,31].

Measurement Model
Let p k and p s k denote the target and sensor position vectors, respectively, at time t k , An IRST sensor measures the bearing and elevation angles of a target [5,8] , as shown in Figure 2. We note that the bearing (φ k ) and elevation ( k ) angles depend on the relative position p k − p s k 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 where φ k and k lie in [0, 2π) and (−π/2, π/2), respectively and the ground range ρ k is defined by
We assume that the measurement noise is zero-mean Gaussian with covariance R n k ∼ N (n k ; 0, R), where σ φ and σ are the bearing and elevation measurement standard deviations (SDs), respectively.

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

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 presents prior mean polar velocitybased state parameters of the target. The NCT motion has a centripetal acceleration s 1 ω 1 of 3 g, where g = 9.8 m 2 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 P 0,p,1 = diag(1000 2 m 2 , 1000 2 m 2 , 30 2 m 2 s −2 , 0.0873 rad 2 , (4.95 × 10 −3 ) 2 rad 2 s −2 , 100 2 m 2 , 5 2 m 2 s −2 ).
Using the Euler approximation, the process noise covariance in the polar velocitybased 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 m 2 s −3 , q ω = 5e − 07 rad 2 s −3 , and q z = 0.001 m 2 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 presents the motion profile of the sensor. In Table 3, ∆t represents the duration of the segment, ∆φ s is the total angular change during the segment, and Ω 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.

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 Figures 5-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 p t j,i andp t j,i 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 where S f is a set of time indices with N f indices, and M is the number of Monte Carlo runs. We have chosen time indices from 51 to 101 to define S f . The RTAMS error [11] for velocity and angular rate are similarly defined. Table 4 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 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 andx 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] 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 Figures 8 and 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.
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 Figures 10 and 11 for the 1 mrad scenario.  Figure 11. RMS angular rate error using the prior variance P 0,p,2 with angle SD of 1 mrad. Table 6 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.

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.

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 XYplane 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 secondorder 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.

Conflicts of Interest:
The authors declare no conflict of interest.