Comparison of Angle-only Filtering Algorithms in 3D using Cartesian ...

Comparison of Angle-only Filtering Algorithms in 3D using Cartesian and Modified Spherical Coordinates Mahendra Mallick1 , Mark Morelande2 , Lyudmila Mihaylova3 , Sanjeev Arulampalam4 , and Yanjun Yan5 1

Propagation Research Associates, Inc., Marietta, GA 30066, USA, [email protected] 2 The University of Melbourne, Parkville VIC 3010, Australia, [email protected] 3 School of Computing and Communications, Lancaster University, United Kingdom, [email protected] 4 Maritime Operations Division, DSTO, Edinburgh SA 5111, Australia, [email protected] 5 ARCON Corporation, Waltham, MA, 02452, USA, [email protected]

Abstract— We compare the performance of the extended Kalman filter (EKF), unscented Kalman filter (UKF), and particle filter (PF) for the angle-only filtering problem in 3D using bearing and elevation measurements from a single maneuvering sensor. These nonlinear filtering algorithms use discrete-time dynamic and measurement models. Two types of coordinate systems are considered, Cartesian coordinates and modified spherical coordinates (MSC) for the relative state vector. The paper presents new algorithms using the UKF and PF with the MSC. We also present an improved filter initialization algorithm. Numerical results from Monte Carlo simulations show that the EKF-MSC and UKF-MSC have the best state estimation accuracy among all nonlinear filters considered and have comparable accuracy with modest computational cost. Keywords: Angle-only filtering in 3D, Modified spherical coordinates (MSC), Nonlinear filtering (NLF), Extended Kalman filter (EKF), Unscented Kalman filter (UKF), Particle filter (PF).

I. I NTRODUCTION The angle-only filtering problem in 3D using bearing and elevation angles from a single maneuvering sensor is the counterpart of the bearing-only filtering problem in 2D [2], [10], [32]. This problem arises in passive ranging using an infrared search and track (IRST) sensor [9], [11], passive sonar, passive radar in the presence of jamming [10], and satellite-to-satellite passive tracking [26], [27]. A great deal of research has been carried out for the bearings-only filtering problem in 2D—see for e.g. [2], [5], [7], [10], [32] and the references therein. However, the number of publications for the angle-only filtering problem in 3D is relatively small [3], [4], [16], [25]– [29], [33], [36], [37]. Early research on the bearing-only filtering problem in 2D used the easy-to-implement discrete-time EKF with relative Cartesian coordinates [1]. The filter used the nearly constant velocity model (NCVM) [8] and nonlinear measurement model for bearing. However, this filter showed poor performance due to premature collapse of the covariance matrix. This led to the formulation of the modified polar coordinates (MPC) [18], [22] in which improved performance of the EKF was demonstrated. The state vector in MPC consists of bearing, bearing-rate,

range-rate divided by range, and the inverse of range [18], [2], [32]. The important difference between the MPC and the Cartesian coordinates is that in MPC, the first three elements of the state are observable even before an ownship maneuver. Decoupling the observable and unobservable components of the state vector prevented ill-conditioning of the covariance matrix and produced better filter performance [18], [22]. The key difficulty of using MPC is that the dynamic model is highly nonlinear. Aidala and Hammel [2] derived exact, closed-form discrete-time stochastic difference equations in MPC using the nonlinear transformations between MPC and relative Cartesian coordinates. They proposed a discrete-time EKF in MPC [2] and showed superior performance relative to the Cartesian EKF [1]. Angle-only filtering in 3D faces the same observability problem [17] that arises in the 2D case [21]. Due to the success of the MPC in the 2D case, Stallard [36] first proposed the modified spherical coordinates (MSC) for the angle-only filtering problem in 3D. Analogous to the log polar coordinates (LPC) [12] in 2D, the log spherical coordinates (LSC) in 3D were proposed in [28]. Numerical results in [28] show that the EKF-MSC and EKF-LSC perform similarly so it is sufficient to consider MSC only in this paper. The components of MSC are elevation, elevation-rate, bearing, bearing-rate times cosine of elevation, range-rate divided by range, and the inverse of range. As with MPC in 2D, the main problem with MSC is the complex nonlinear dynamic model. Thus, calculation of the predicted state estimate and covariance in MSC is challenging. Since the bearing and elevation are components of the MSC, the update step in MSC is straightforward [8]. The discrete-time dynamic model in MSC can be derived in two different ways. Li et al [26] derived closed form analytic expressions for the discrete-time nonlinear dynamic model in MSC using an approach similar to that in [2] for MPC. This model can be used to calculate the predicted state estimate in MSC. However, they do not present an algorithm for calculating the predicted covariance in MSC. In the second approach, the discrete-time dynamic model in MSC can be derived by a sequence of transformations. Suppose we have the MSC at time tk−1 . First, the MSC are

1392

transformed to relative Cartesian coordinates at time tk−1 . Then the relative Cartesian coordinates at time tk are obtained by using the NCVM for relative Cartesian coordinates during the time interval [tk−1 , tk ]. Finally, the relative Cartesian coordinates are transformed to MSC at time tk . Stallard [36] used the second approach to compute the predicted state estimate. However, calculation of the predicted covariance in [36] was based on a linearization of the dynamic equation which operated under the assumption that the relative geometry between the target and ownship does not change significantly during the interval [tk−1 , tk ]. The underlying Cartesian dynamic model is the Singer model [35]. A similar method is adopted in [3], [4]. In [25], the EKF is implemented using a discretized linear approximation for both the predicted state estimate and covariance. Karlsson and Gustafsson [25] implemented a PF [6], [15], [32] for MSC using a multistep Euler approximation. In [28], exact SDEs for MSC and LSC were derived first from the NCVM in 3D for the relative Cartesian state vector. Then EKFs were implemented for MSC and LSC by numerically integrating nonlinear differential equations for the predicted state estimate and covariance matrix jointly. This approach represents the continuous-discrete filtering (CDF). In this paper, we calculate the predicted state estimate and covariance using the second approach for the exact discretetime dynamic model in MSC. Although the same dynamic model is used in [3], [4], [36], its usage in their EKF implementations does not properly account for the process noise. In particular, the approximate method used in [3], [4], [36] to calculate the predicted covariance is only valid over time intervals for which the relative geometry between the target and ownship does not change significantly. We consider two classes of filtering algorithms. Each class considers the relative state vector and uses a discrete-time measurement model for bearing and elevation. The first class of algorithms uses the relative Cartesian state vector with the discrete-time NCVM in 3D, and hence the measurement model is nonlinear. The second class uses the exact discretetime dynamic model in MSC. Thus, the measurement model is linear. An EKF [8], [14], an UKF [23], [24], [32] and a bootstrap filter (BF) [6], [15], [32] are developed for each class. The UKF and PF algorithms using MSC represent new algorithms. We don’t use the assumption that the relative geometry between the target and ownship is slowly varying. We perform Monte Carlo simulations to compare the accuracy and computational complexity of various algorithms. The paper is organized as follows. Section II defines the tracker and sensor coordinate frames and describes different coordinate systems for the target and ownship. Section III presents the dynamic models of the target and ownship, whereas the measurement models for the Cartesian relative state and MSC are presented in Section IV. An improved filter initialization algorithm is described in Section V. Nonlinear filtering using relative Cartesian coordinates is presented in Section VI, which describes Cartesian EKF (CEKF), Cartesian UKF (CUKF), and Cartesian BF (CBF), Section VII describes NLF using MSC which includes EKF-

MSC, UKF-MSC, and BF-MSC. Numerical simulations and results are described in Section VIII and conclusions are summarized in Section IX. II. C OORDINATE S YSTEMS FOR TARGET AND OWNSHIP The origin of the tracker coordinate frame (T frame) is specified by the geodetic longitude λ0 , geodetic latitude φ0 , and geodetic height h0 . The X, Y , and Z axes of the T frame are along the local east, north, and upward directions, respectively, as shown in Figure 1. The state of the target is defined in the T frame. The motion of the ownship is assumed to be deterministic and the state of the ownship is assumed to be known precisely. The ownship performs maneuvers so that the target state becomes observable. Since we use standard conventions for coordinate frames and angle variables, the rotational transformation TST from the T frame to the sensor frame (S frame) is defined differently in our approach compared with that in [36]. Local Up

ZT

Target

z

ε

y

Ownship

YT

β

x

Local North

Local East

XT

Fig. 1. Definition of tracker coordinate frame (T frame), bearing β ∈ [0, 2π] and elevation angle  ∈ [−π/2, π/2].

A. Cartesian Coordinates for State Vector and Relative State Vector The Cartesian states of the target and ownship are defined in the T frame, respectively, by  0 xt := xt y t z t x˙ t y˙ t z˙ t , (1) and xo :=



xo

yo

zo

x˙ o

y˙ o

z˙ o

0

.

(2)

The relative state vector in the T frame is defined by x := xt − xo .

(3)

Let x = [x y z x˙ y˙ z] ˙ 0 denote the relative state vector t in the T frame. Then x = x − xo , x˙ = x˙ t − x˙ o , etc. Let rT denote the range vector of the target from the ownship (or sensor) in the T frame. Then rT is defined by  0  t 0 rT := x y z = x − xo y t − y o z t − z o . (4)

1393

The range is defined by T

r := kr k =

t wk−1

p

x2

+

y2

+

z2,

r > 0.

(5)

The range vector can be expressed in terms of range, bearing (β) and elevation (), as defined in Figure 1, by   cos  sin β rT = r  cos  cos β  , β ∈ [0, 2π],  ∈ [−π/2, π/2]. sin  (6) The ground range is defined by p (7) ρ := x2 + y 2 = r cos , ρ > 0. B. Modified Spherical Coordinates for Relative State Vector Following Stallard’s convention [36], we use ω as a component of the MSC, where ˙ cos (t). ω(t) := β(t)

(8)

Let ζ(t) denote the logarithm of range r(t) ζ(t) := ln r(t).

Qk−1

(15)

t t wk−1 ∼ N (wk−1 ; 0, Qk−1 ),  3  ∆k /3 ∆2k /2 = ⊗ diag(qx , qy , qz ), ∆2k /2 ∆k

(16) (17)

where qx , qy , qz are the power spectral densities of the process noise [14] along the X, Y , and Z axes of the T frame, respectively. Since the motion of the ownship is assumed to be deterministic, the process noise is not used for the dynamic model of the ownship. The dynamic models of the ownship for the CV and CT are described, respectively, by xok = Fk−1 xok−1 , xok

(10)

CT

=F

(∆k , ω)xok−1 ,

(18) (19)

where (11)

The relative state vector of the target in MSC is defined by [28], [36]  0 ξ(t) := ξ1 (t) ξ2 (t) ξ3 (t) ξ4 (t) ξ5 (t) ξ6 (t)  0 ˙ = ω(t) (t) ˙ ζ(t) β(t) (t) 1/r(t) . (12) The components of the MSC defined in [36] and [28] are the same; however, the ordering is different. III. DYNAMIC M ODELS We present discrete-time dynamic models of the target and ownship in the T frame first. Next, we present the dynamic model of the target relative to the ownship in the T frame. The target follows the NCVM in 3D. The ownship follows a sequence of constant velocity (CV) and coordinated turn (CT) models in a plane parallel to the XY plane of the T frame. A. Dynamic Model for State Vector and Relative State Vector in Cartesian Coordinates Let xtk denote the Cartesian state of the target at time tk . Then the discrete-time dynamic model of the target is described by [8], [14] t xtk = Fk−1 xtk−1 + wk−1 ,

F(tk − t)wt (t) dt,

where ∆k := tk − tk−1 . In (14), ⊗ refers to the Kronecker product [19]. Using the properties of the continuous-time process noise wt (t), state transition matrix Fk−1 defined in t (14), and the definition of wk−1 in (15), we can show that t wk−1 is a zero-mean Gaussian white integrated process noise with covariance Qk−1

(9)

Differentiating (9) with respect to time, we get ˙ = r(t)/r(t). ζ(t) ˙

tk

:= tk−1

Then r(t) = exp[ζ(t)].

Z

FCT (∆, ω) =  1 0 0 sin(ω∆)/ω  0 1 0 [1 − cos(ω∆)]/ω   0 0 1 0   0 0 0 cos(ω∆)   0 0 0 sin(ω∆) 0 0 0 0

−[1 − cos(ω∆)]/ω sin(ω∆)/ω 0 − sin(ω∆) cos(ω∆) 0

 0 0   ∆  . 0   0  1 (20)

Using (13) in the definition (3) of the relative state vector and then adding and subtracting Fk−1 xok−1 , we get t xk = Fk−1 xk−1 + wk−1 − uk−1 ,

(21)

uk−1 := xok − Fk−1 xok−1 .

(22)

where We note that when ωko approaches zero, FCT (∆k , ωko ) reduces to Fk−1 and uk−1 becomes zero. B. Dynamic Model for Relative State Vector in Modified Spherical Coordinates Let fCMSC : R6 → R6 denote the transformation from C relative Cartesian coordinates to MSC. Similarly, let fMSC : 6 6 R → R denote the inverse transformation from MSC to relative Cartesian coordinates. Then,

(13)

ξ k = fCMSC (xk ),

(23)

where Fk−1 and are the state transition matrix [8], [14] and integrated process noise [14], respectively, for the time interval [tk−1 , tk ],   1 ∆k Fk−1 = F(tk , tk−1 ) := ⊗ I3 , (14) 0 1

C fMSC (ξ k ).

(24)

t wk−1

xk =

C Functional forms of fCMSC (xk ) and fMSC (ξ k ) are presented in [29]. Then, using (21) in (23), we get

1394

t ξ k = fCMSC (Fk−1 xk−1 + wk−1 − uk−1 ).

(25)

We have

where C xk−1 = fMSC (ξ k−1 ).

(26)

φˆ1 =

C t ξ k = fCMSC (Fk−1 fMSC (ξ k−1 ) + wk−1 − uk−1 ).

(27)

Formally, we can write (27) as t ξ k = b(ξ k−1 , uk−1 , wk−1 ),

(28)

t where b is a nonlinear function of ξ k−1 , uk−1 , and wk−1 . We note that a closed form analytic expression for the nonlinear function b is difficult to obtain. However, it is straightforward to calculate the predicted state estimate ξˆk|k−1 approximately given ξˆk−1|k−1 , using nested functions in (27). It can be t seen from (28) that the process noise wk−1 is nonlinearly transformed in the MSC dynamic model. As will be seen in Section VII, this makes the EKF and UKF slightly more complicated compared to the usual dynamic models in which the process noise is additive.

The passive sensor collects bearing and elevation measurements {zk } at discrete times {tk }. The measurement model for the bearing and elevation angles using the relative Cartesian state vector xk is zk = h(xk ) + nk ,

(29)

where βk k



 =

tan−1 (xk , yk ) tan−1 (zk , ρk )

 ,

(30)

where the ground range ρk is defined in (7) and nk is a zeromean white Gaussian measurement noise with covariance R R := diag(σβ2 , σ2 ).

nk ∼ N (nk ; 0, R),

r¯ s¯ α ¯

γ¯

0

,

(35) (36)

The distribution (34) forms the basis for filter initialization of the angle-only filtering algorithms in Cartesian coordinates and MSC. A. Initialization of Relative Cartesian Coordinates Initialization of the EKF and UKF requires a mean and covariance matrix. Given a vector φ1 distributed according to (34) the mean and covariance matrix of the target state in relative Cartesian coordinates are Z ˆ , Σ1 ) dφ, ˆ 1 = u(φ)N (φ; φ x (37) 1 Z ˆ , Σ1 ) dφ, (38) ˆ 1 ][u(φ) − x ˆ 1 ]0 N (φ; φ P1 = [u(φ) − x 1 where

IV. M EASUREMENT M ODELS



z01

Σ1 = diag(σβ2 , σ2 , σr2 , σs2 , σα2 , σγ2 ).

Substitution of (26) in (25) gives

h(xk ) :=



    u(φ) =    

r cos  sin β r cos  cos β r sin  s cos γ sin α − x˙ o1 s cos γ cos α − y˙ 1o s sin γ − z˙1o

    .   

(39)

Note that most approaches to angle only filtering, in both 2D and 3D, use linearized approximations to the integrals (37) and (38). Exact evaluation of the integrals is presented in [29]. The PF is initiated with a collection of samples which can be found as xi1 = u(φi ) where φi ∼ N (· ; φˆ1 , Σ1 ) for i = 1, . . . , n. The sample weights are w1i = 1/n, i = 1, . . . , n. B. Initialization of Modified Spherical Coordinates The first three components of the MSC are given by

(31) ξ1 = ω = (y x˙ − xy)/ρr, ˙

(40)

The measurement model for the bearing and elevation angles βk , k using MSC is

2 ξ2 = ˙ = (zρ ˙ 2 − z(xx˙ + y y)/ρr ˙ ,

(41)

zk = Hξ k + nk ,

ξ3 = ζ˙ = (xx˙ + y y˙ + z z)/r ˙ .

(42)

 H :=

0 0

0 0

0 0

1 0

0 1

2

(32) 0 0

 .

Using (4)-(7) in (40)-(42), we get     x˙ ξ1 1  ξ2  = A(β, )  y˙  , r z˙ ξ3

(33)

V. F ILTER I NITIALIZATION We initialize the filter using the first measurement z1 at t1 and prior information on range and velocity of the target. The target state at any time can be completely defined by the vector φ := [β, , r, s, α, γ]0 where s is speed, α, and γ are the bearing and elevation of target velocity. Let φ1 denote the vector φ at time t1 . Let r¯, s¯, α ¯ , and γ¯ denote the prior mean for range, speed, α and γ, respectively. Similarly, let σr2 , σs2 , σα2 , and σγ2 denote the prior variances for range, speed, α and γ, respectively. Then the distribution of φ1 given the measurement z1 and prior information on the range and velocity of the target is φ1 |z1 ∼ N (· ; φˆ1 , Σ1 ),

(34)

(43)

where 

cos β A(β, ) :=  − sin β sin  sin β cos 

 − sin β 0 − cos β sin  cos   . cos β cos  sin 

(44)

The mean and covariance matrix of the relative target state in MSC given a vector φ distributed according to (34) are Z ξˆ1 = v(φ)N (φ; φˆ1 , Σ1 ) dφ, (45) Z P1 = [v(φ) − ξˆ ][v(φ) − ξˆ ]0 N (φ; φˆ , Σ1 ) dφ, (46)

1395

1

1

1

where

VII. N ONLINEAR F ILTERING USING MSC 



0  0       s cos γ sin α − x˙ o1   0  1 A(β, )   s cos γ cos α − y˙ 1o  . v(φ) =   β + r 03   s sin γ − z˙1o    1/r (47) The integrals (45) and (46) can be evaluated over all variables except for the range r. Integration over r can be done numerically using, for example, Monte Carlo approximation. Due to lack of space, the expressions the components of ξˆ1 and P1 are not presented in the paper but can be found in [29]. VI. N ONLINEAR F ILTERING USING R ELATIVE C ARTESIAN C OORDINATES The dynamic model (NCVM in 3D) is linear and the measurement model for bearing and elevation is nonlinear for this case. The widely used EKF is based on linearized approximations to nonlinear dynamic and/or measurement models [8], [14]. For this case, the linearized approximation is performed in the measurement update step. The details of the algorithm are described in [8], [14]. The UKF, like the EKF, is also an approximate filtering algorithm. However, instead of using the linearized approximation, the UKF uses the unscented transformation (UT) to approximate the moments [23], [24]. This approach has two advantages over linearization: it avoids the need to calculate the Jacobian and it provides a more accurate approximation. The details of the UKF algorithm are described in [23], [24] and [32]. Particle filters are a class of sequential Monte Carlo methods for approximating the posterior density of the target state. The most common form of PF adopts a sequential importance sampling (SIS) [15], [6], [32] approach in which samples of the target state are drawn from an importance density and weighted appropriately each time a measurement is acquired. We use a regularised bootstrap filter (BF). This involves first drawing samples from the prior and weighting the samples by their likelihood [6], [15], [32]. Regularization is then performed, as suggested in [20], [30], by drawing from a kernel density approximation using a Gaussian kernel with covariance matrix ˆ k−1 , Ωk = b(n)P

The dynamic model using MSC is nonlinear as in (27) or (28). In addition, the process noise is not additive. Since, the bearing and elevation are elements of the MSC, the measurement model (32) is linear. The EKF using MSC is described by Algorithm 1. We note that linearization of the dynamic model is performed over t both the previous state ξ k−1 and the process noise wk−1 . As a result, the Jacobian B is a 6 × 12 matrix rather than the 6 × 6 matrix which would result if the dynamic model were nonlinear in the previous state and linear in the additive process noise. A recursion of the UKF using MSC is given by Algorithm 2. In Algorithm 2, the nonlinear transformation is applied to a 12-dimensional random variable. Therefore, 2 × 12 + 1 = 25 sigma points are required. The weights are selected as w0 = κ/(12 + κ) and wi = 1/[2(12 + κ)], i = 1, . . . , 24 with κ = −3. Algorithm 3 presents a recursions of the PF using MSC. This PF provides state estimates in relative Cartesian coordinates by transforming each sample from MSC to relative Cartesian coordinates and computing the weighted mean. Algorithm 1: A recursion of the EKF using MSC. Input: posterior mean ξˆk−1|k−1 and covariance matrix Pk−1|k−1 at time tk−1 and the measurement zk . Output: posterior mean ξˆk|k and covariance matrix Pk|k at time tk . 0 set ξˆk|k−1 = ξˆk−1|k−1 and P0k|k−1 = Pk−1|k−1 . compute the Jacobian B =   Dξ0 b(ξ, uk−1 , w) Dw0 b(ξ, uk−1 , w) ξ=ξˆ . ,w=0 k−1|k−1

compute the predicted statistics 0 ξˆk|k−1 = b(ξˆk|k−1 , uk−1 , 0),

Pk|k−1 = B diag(P0k|k−1 , Q(∆k ))B0 . compute the innovation covariance Sk = HPk|k−1 H0 + R. compute the gain matrix Kk = Pk|k−1 H0 S−1 k . compute the posterior statistics ξˆk|k = ξˆk|k−1 + Kk (zk − Hξˆk|k−1 ), Pk|k = Pk|k−1 − Kk Sk K0k .

(48)

ˆ k−1 is the weighted sample where b(n) is a scaling factor and P covariance matrix. For samples drawn from a Gaussian distribution, the mean integrated squared error of the kernel density estimator is minimized by selecting b(n) = (2n)−1/5 [34]. We have found that better results are obtained using a smaller scaling factor. In particular, we use b(n) = (2n)−1/5 /16. Such a reduction is suggested in [34] for samples from a multimodal distribution.

VIII. N UMERICAL S IMULATIONS AND R ESULTS The scenario used in our simulation is similar to that used in [11]. We have made some changes to make the scenario three dimensional in nature. Initial height z1o of the ownship is 10.0 km. Target’s initial ground range ρ1 , bearing β1 , and height z1t are shown in Table I. Then the initial elevation 1 can be calculated. Table I also shows target’s initial speed s1 , course c1 in the XY -plane and the Z component of velocity

1396

Algorithm 2: A recursion of the UKF using MSC. Input: posterior mean ξˆk−1|k−1 and covariance matrix Pk−1|k−1 at time k − 1 and the measurement zk Output: posterior mean ξˆk|k and covariance matrix Pk|k at time k construct the matrices  p Σ = 06,1 (12 + κ)Pk−1|k−1 06,6  p − (12 + κ)Pk−1|k−1 06,6 p  Ω = 06,1 06,6 (12 + κ)Q(∆k ) p  06,6 − (12 + κ)Q(∆k )

Algorithm 3: A recursion of the BF using MSC. i Input: a weighted sample {wk−1 , ξ ik−1 } from the posterior at time tk−1 and the measurement zk . Output: a weighted sample {wki , ξ ik } from the posterior at time tk . select indices c(1), . . . , c(n) according to the weights 1 n , . . . , wk−1 . wk−1 for i = 1, . . . , n do draw wki ∼ N (0, Q(∆k )) c(i) set ξ ik = b(ξ k−1 , uk , wki ) compute the un-normalized weight w ˜ki = N (zk ; Hξ ik , R) end compute the normalized weights , n X j i i wk = w ˜k w ˜k .

for b = 0, . . . , 24 do compute the sigma points Ξk,b = ξˆk−1|k−1 + σ b , Wk,b = ω b where σ b and ω b are the (b + 1)th columns of Σi and Ω, respectively. compute the transformed sigma point and covariance matrix: Ek,b = b(Ξk,b , uk−1 , Wk,b )

j=1

compute the state estimate

end compute the predicted statistics ξˆk|k−1 =

24 X

ˆk = x

Pk|k−1 =

C wki fMSC (ξ ik ).

i=1

wb Ek,b

b=0 24 X

n X

TABLE I I NITIAL PARAMETERS OF TARGET.

wb (Ek,b − ξˆk|k−1 )(Ek,b − ξˆk|k−1 )0

Variable ρ1 (km) β1 (deg) 1 (deg) z1t (km) s1 (m/s) c1 (deg) z˙1t (m/s)

b=0

compute the innovation covariance Sk = HPk|k−1 H0 + R. compute the gain matrix Kk = Pk|k−1 H0 S−1 k . compute the corrected statistics ξˆk|k = ξˆk|k−1 + Kk (zk − Hξˆk|k−1 )

Value 138.0 45.0 -0.415 9.0 297.0 -135.0 0.0

Pk|k = Pk|k−1 − Kk Sk K0k .

z˙1t . Target’s initial position and velocity in √ Cartesian√coordinates can be found from Table I as (138/ 2, 138/ 2, 9) √ km and 297/ 2(−1, −1, 0) m/s, respectively. The motion of the target is governed by the NCVM. The power spectral densities (qx , qy , qz ) of the zero-mean white acceleration process noise along the X, Y , and Z axes of the T frame are (0.01, 0.01, 0.0001) m2 s−3 , respectively. The ownship moves in a plane parallel to the XY -plane at a fixed height of 10 km with segments of CV and CT. The profile of the ownship motion is presented in Table II. In Table II, ∆t represents the duration of the segment, ∆φ is the total angular change during the segment, and ω o is the angular velocity of the ownship during the segment. The ownship trajectory and the true target trajectory from the first Monte Carlo run are shown in Figure 2. The measurement sampling time interval is 1.0 s. The

bearing and elevation measurement error standard deviations used in our simulation were 1, 15 and 35 mrad (0.057, 0.857 and 2 degrees), representing high, medium, and low measurement accuracy, respectively. We used 1000 Monte Carlo simulations to calculate various statistics such as the root mean square (RMS) position and velocity errors. The filters are initialized as described in Section V with the parameters shown in Table I. All PFs are implemented with a sample size of 10,000. Two measures of performance are used to compare the algorithms described in Sections VI-VII. The first is the RMS position error averaged from the end of the last observer maneuver to the end of the surveillance period. The second performance measure is the RMS position error at the end of the surveillance period. The results are shown in Tables III and IV. The execution times of the algorithms are given in Table V. There are several points of interest to discuss. The best performance is achieved by the EKF-MSC and UKF-MSC. The degree of their superiority over the other algorithms

1397

TABLE II

TABLE IV

M OTION PROFILE OF OWNSHIP FOR VARIOUS SEGMENTS . Time Interval (s) [[0, 15] [15, 31] [31, 43] [43, 75] [75, 86] [86, 102] [102, 210]

∆φ (rad) 0 −π/4 0 π/2 0 −π/4 0

Motion Type CV CT CV CT CV CT CV

ωo (rad/s) 0 −π/64 0 π/64 0 −π/64 0

Algorithm CEKF CUKF CBF EKF-MSC UKF-MSC BF-MSC

x 10

Target Ownship

9 8 7 6

Y (m)

POSITION ERROR IN METERS .

sdv (mrad) 1.0 0.472 0.468 0.527 0.462 0.462 1.355

sdv (mrad) 15.0 3.363 3.308 4.173 2.413 2.533 3.575

sdv (mrad) 35.0 4.780 4.658 5.384 3.333 3.499 5.286

of the PFs could be improved by using a higher number of particles, but this would obviously increase their already large computational cost. An interesting aspect of the results is that the BF does not benefit from the use of MSC to the same extent as the EKF and UKF. When the measurement accuracy is high (e.g. a standard deviation of 1 mrad), the RMS position error for the CBF is about half of that for the BF-MSC. For higher measurement standard deviations (e.g. 15, 35 mrad), the BF-MSC performs slightly better than the CBF with nearly the same computational cost.

Target and Ownship Trajectories

4

10

∆t (s) 15 16 12 32 11 16 108

F INAL RMS

5 4 3

TABLE V

2

CPU 1 0

TIMES OF FILTERING ALGORITHMS FOR THE MEASUREMENT STANDARD DEVIATION OF

0

2

4

6

8

10

X (m) Fig. 2.

Algorithm CEKF CUKF CBF EKF-MSC UKF-MSC BF-MSC

4

x 10

Target and ownship trajectories.

depends on the the measurement accuracy. For high measurement accuracy, the EKF-MSC and UKF-MSC are only marginally better than the EKF and UKF using relative Cartesian coordinates. For low measurement accuracy, the performance improvements of the EKF-MSC and UKF-MSC are more pronounced. These performance improvements are offset somewhat by the larger computational cost of using MSC compared to relative Cartesian coordinates, as seen in Table V. Although more expensive than their Cartesian counterparts, the EKF-MSC and UKF-MSC do not pose a severe computational burden. This is certainly true when compared to two PF algorithms. In addition to having a much greater computational cost, the PFs also perform worse than the EKF-MSC and UKF-MSC, much worse in some cases. The poor performance TABLE III T IME - AVERAGED RMS POSITION ERROR IN METERS . Algorithm CEKF CUKF CBF EKF-MSC UKF-MSC BF-MSC

sdv (mrad) 1.0 0.650 0.644 0.694 0.629 0.629 1.310

sdv (mrad) 15.0 5.786 5.743 6.183 4.773 4.827 5.689

sdv (mrad) 35.0 12.00 12.02 12.09 10.65 10.48 11.71

CPU (s) 0.036 0.110 6.280 0.980 0.820 15.600

15

MRAD .

Relative CPU 1.00 2.97 175.22 27.39 22.86 434.90

IX. C ONCLUSIONS We have considered the angle-only filtering problem in 3D of a non-maneuvering target using bearing and elevation measurements. The paper presents two classes of algorithms based on relative Cartesian coordinates and modified spherical coordinates (MSC). MSC are the 3D analogue of the wellknown modified polar coordinates (MPC) in 2D. These coordinate systems have the important property of decoupling the potentially unobservable range from other observable elements of the state vector. As in the case of 2D angle-only filtering, our results for the 3D case show that using a coordinate system which decouples observable and non-observable components is desirable, although the benefits of doing this were modest in our scenario. Of the algorithms considered here, computationally efficient Gaussian approximations, such as the EKF and UKF, were the most effective. They provided accurate state estimates at a fraction of the expense of the various PF implementations. The EKF-MSC and UKF-MSC provide the best filtering performance for the scenario considered. Our future work will consider the realistic scenario of a maneuvering target. We shall also calculate the posterior Cram´er Rao lower bound (PCRLB) for the filtering problem which represents the best achievable accuracy [13]. Then the

1398

accuracy of various filtering algorithms can be compared with the PCRLB. ACKNOWLEDGMENT The authors thank Sam Blackman for providing the details of the tracking scenario and useful discussions and advice. The authors also thank Anand Mallick for verifying the derivations using Mathematica, developing Matlab code, and generating numerical results. R EFERENCES [1] V. J. Aidala, Kalman filter behavior in bearing-only tracking applications, IEEE Trans. on Aerospace and Electronic Systems, Vol. AES-15, No. 1, pp:29–39, January 1979. [2] V. J. Aidala and S. E. Hammel, Utilization of modified polar coordinates for bearings-only tracking, IEEE Transactions on Automatic Control, 28(3):283–294, 1983. [3] R. R. Allen and S. S. Blackman, Implementation of an angle-only tracking filter, Proc. of SPIE, Signal and Data Processing of Small Targets, Vol. 1481, Orlando, FL, USA, April 1991. [4] R. R. Allen and S. S. Blackman, Angle-only tracking with a MSC filter, Proc. of Digital Avionics Systems Conference, pp. 561 – 566, 1991. [5] S. Arulampalam and B. Ristic, Comparison of the Particle Filter with Range-Parameterised and Modified Polar EKFs for Angle-Only Tracking, Proc. of SPIE, Vol. 4048, 2000. [6] M. Arulampalam, S. Maskell, N. Gordon and T. Clapp, A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking, IEEE Trans. on Signal Processing, Vol. SP-50, No. 2, pp. 174–188, February 2002 [7] S. Arulampalam, M. Clark, and R. Vinter, Performance of the Shifted Rayleigh Filter in Single-sensor Bearings-only Tracking, Proc. of the Tenth International Conf. on Information Fusion, Qu´ebec, Canada, July 2007. [8] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and Navigation, John Wiley & Sons, 2001. [9] S. S. Blackman and S. H. Roszkowski, Application of IMM Filtering to Passive Ranging, In Proc. of SPIE, 3809:270–281, 1999. [10] S. Blackman and R. Popoli, Design and Analysis of Modern Tracking Systems, Artech House, 1999. [11] S. S. Blackman, T. White, B. Blyth, and C. Durand, Integration of Passive Ranging with Multiple Hypothesis Tracking (MHT) for Application with Angle-Only Measurements, Proc. of SPIE, Vol. 7698, pp. 769815-1 – 769815-11, 2010. [12] T. Br´ehard and J-P. Le Cadre, Closed-form Posterior Cram´er-Rao Bound for a Manoeuvring Target in the Bearings Only Tracking Context Using Best-Fitting Gaussian Distribution, Proc. of the Ninth International Conf. on Information Fusion, Florence, Italy, July 2006. [13] T. Br´ehard and J-P. Le Cadre, Closed-form posterior Cram´er-Rao bounds for bearings only tracking, IEEE Trans. on Aerospace and Electronic Systems, Vol. AES-42, No. 4, pp. 1198–1223, October 2006. [14] A. Gelb, editor, Applied Optimal Estimation, MIT Press, 1974. [15] N. J. Gordon, D. J. Salmond, and A. F. M. Smith, Novel approach to nonlinear/non-Gaussian Bayesian state estimation, IEE Proceedings-F, Vol. 140, No. 2, pp. 107–113, April 1993. [16] P. Gurfil and N. J. Kasdin, Two-step optimal estimator for three dimensional target tracking, IEEE Trans. on Aerospace and Electronic Systems, Vol. AES-40, No. 3, pp. 780 –793, July 2005. [17] S. E. Hammel and V. J. Aidala, Observability Requirements for Three-Dimensional Tracking via Angle Measurements, IEEE Trans. on Aerospace and Electronic Systems, Vol. AES-24, No. 2, pp. 200 –207, March 1985.

[18] H. D. Hoelzer, G. W.Johnson, and A. O.Cohen, Modified Polar Coordinates - The Key to Well Behaved Bearings Only Ranging, In IR & D Report 78-M19-OOOlA, IBM Federal Systems Division, Shipboard and Defense Systems, Manassas, VA 22110, August 31, 1978. [19] R. A. Horn and C. R.Johnson, Topics in Matrix Analysis, Cambridge University Press, 1991. [20] M. H¨urzeler and H. R. K¨unsch, Monte carlo approximations for general state-space models, Journal of Computational and Graphical Statistics, Vol. 7, No. 2, pp. 175–193, 1998. [21] C. Jauffret and D. Pillon Observability in passive target motion analysis, IEEE Trans. on Aerospace and Electronic Systems, Vol. AES-32, No. 4, pp. 1290–1300, October 1996. [22] G. W. Johnson, H. D. Hoelzer, A. O. Cohen, and E. F. Harrold, Improved coordinates for target tracking from time delay information, Proc. of the Time Delay Estimation and Applications Conference, Naval Postgraduate School, Monterey, CA, May 1979. vol. 2. pp. M1–M32. [23] S. Julier, J. Uhlmann and H.F. Durrant-Whyte, A new method for the nonlinear transformation of means and covariances in filters and estimators, IEEE Trans. on Automatic Control, Vol. AC-45, No. 3, pp. 477–482, March 2000. [24] S. J. Julier and J. K. Uhlmann, Unscented filtering and nonlinear estimation, Proc. of the IEEE, Vol. 92, No. 3, pp. 401–422, March 2004. [25] R. Karlsson and F. Gustafsson, Range estimation using angle-only target tracking with particle filters, Proc. American Control Conference, pp. 3743 – 3748, 2001. [26] Q. Li, F. Guo, Y. Zhou, and W. Jiang, Observability of Satellite to Satellite Passive Tracking from Angles Measurements, Proc. IEEE International Conference on Control and Automation, pp. 1926 – 1931, 2007. [27] Q. Li, L. Shi, H. Wang, and F. Guo, Utilization of Modified Spherical Coordinates for Satellite to Satellite Bearings-Only Tracking, Chin. J. Space Sci., Vol. 29, No. 6, pp. 627–634, 2009. [28] M. Mallick, L. Mihaylova, S. Arulampalam, and Y. Yan, Angle-only Filtering in 3D Using Modified Spherical and Log Spherical Coordinates, In 2011 International Conference on Information Fusion, Chicago, USA, July 5-8, 2011. [29] M. Mallick, M. Morelande, L. Mihaylova, S. Arulampalam, and Y. Yan, Angle-only Filtering in 3D, Chapter 1, In Integrated Tracking, Classification, and Sensor Management: Theory and Applications, M. Mallick, V. Krishnamurthy, and B-N Vo, Ed., Wiley-IEEE, 2012 (to be published). [30] C. Musso, N. Ouddjane, and F. Le Gland, Improving Regularised Particle Filters, Chapter 12, in Sequential Monte Carlo Methods in Practice, (Ed.) A. Doucet, N. De Freitas, N. Gordon, Springer, pp. 247271, 2001. [31] M. K. Pitt and N. Shephard, Filtering via Simulation:Auxiliary particle filters, Journal of the American Statistical Association, Vol. 94, No. 446, pp. 590–599, 1999. [32] B. Ristic, S. Arulampalam, and N. Gordon, Beyond the Kalman Filter, Artech House, 2004. [33] P. N. Robinson, Modified spherical coordinates for radar, Proc. of the AIAA Guidance, Navigation and Control Conference, Scottsdale, AZ, pp. 55–64, Aug. 1-3, 1994. [34] B. W. Silverman, Density Estimation for Statistics and Data Analysis, Chapman and Hall, 1986. [35] R. A. Singer, Estimating Optimal Tracking Filter Performance for Manned Maneuvering Targets, IEEE Trans. on Aerospace and Electronic Systems, Vol. AES-6, pp: 473–483, July 1970. [36] D. V. Stallard, An angle-only tracking filter in modified spherical coordinates, Proc. of the AIAA Guidance, Navigation and Control Conference, Monterey, CA, pp. 542–550, Aug. 17-19, 1987. [37] D. V. Stallard, Angle-only tracking filter in modified spherical coordinates, Journal of Guidance, Control, and Dynamics, Vol. 14, No. 3, pp. 694–696, 1991.

1399