Continuous-Discrete Filtering using EKF, UKF, and ... - Semantic Scholar

Continuous-Discrete Filtering using EKF, UKF, and PF Mahendra Mallick1 , Mark Morelande2 , Lyudmila Mihaylova3 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]

Abstract—Continuous-discrete filtering (CDF) arises in many real-world problems such as ballistic projectile tracking, ballistic missile tracking, bearing-only tracking in 2D, angle-only tracking in 3D, and satellite orbit determination. We develop CDF algorithms using the extended Kalman filter (EKF), unscented Kalman filter (UKF), and particle filter (PF) with applications to the angle-only tracking in 3D. The modified spherical coordinates are used to represent the target state. Monte Carlo simulations are performed to compare the performance and computational complexity of the proposed filtering algorithms. Our results show that the CDF algorithms based on the EKF and UKF have the best state estimation accuracy and nearly the same computational cost. Keywords: Continuous-discrete filtering (CDF), Angle-only filtering in 3D, Modified spherical coordinates (MSC), Continuousdiscrete Extended Kalman filter, Continuous-discrete Unscented Kalman filter, Continuous-discrete Particle filter.

I. I NTRODUCTION The motion of macroscopic objects (e.g. cars, airplanes, ballistic missiles, satellites, planets, etc) is governed by the Newton’s laws of motion [12] (or equivalently Lagrange’s or Hamilton’s equations [10]) in the non-relativistic domain. The special theory of relativity [25] governs the motion of non-massive macroscopic objects in the relativistic domain. Microscopic objects such as electrons and atoms follow the laws of quantum mechanics (e.g. Schr¨odinger equation in the non-relativistic domain [31] or Dirac equation in the relativistic domain [7]). In tracking applications, we usually deal with macroscopic objects in the non-relativistic domain and two types of equations of motion are used; kinematic equations and dynamic equations. Equations for constant velocity (CV), constant acceleration(CA), and coordinated turn represent kinematic equations. The motion of a ballistic missile [8], [23] satisfies Newton’s second law [12], which states that the rate of change of momentum is equal to the total external force acting on the object in an inertial coordinate frame. An inertial coordinate frame is a coordinate frame which is fixed or moving with a constant velocity relative to a star. In an inertial coordinate frame, only real forces such as the force due to gravity, atmospheric drag, and thrust, etc. are included in Newton’s second law. If we use a non-inertial coordinate frame, such as the Earth-centered Earth-fixed (ECEF) World Geodetic System 1984 (WGS84) Cartesian coordinate frame

(non-inertial due to rotation and acceleration relative to a star), then pseudo-forces such as the centrifugal and Coriolis forces must be included. Although Newton’s second law is exact in its domain of application, it is quite difficult to specify the actual forces acting on an object. For example, the gravitational force acting on an Earth-orbiting satellite can be only approximately specified by using a spherical harmonic expansion of the Earth’s gravitational potential of a given order an degree [33]. Similarly, the drag force cannot be specified exactly due to the complex geometric structure of a satellite and the dynamic variation of the atmospheric density. Most atmospheric density models [28] are static in nature. Similar comment applies to the solar radiation force [33] acting a satellite - it cannot be specified exactly. Therefore, forces acting on a ballistic missile [8], [23] or ballistic projectile [24] cannot be specified exactly. Thus, there is always a model mismatch between the actual forces acting on a ballistic projectile, ballistic missile, or satellite and the forces used in our model. The Bayesian framework is a robust and natural framework, where we use a stochastic differential equation (SDE) to describe the equation of motion of an object, in which the continuoustime process noise represents the effect of model mismatch. Noisy measurements from one or more sensors are available at discrete times for estimating the state of an object. Therefore, the filtering problem for this class of problems is a continuousdiscrete filtering (CDF) problem [9], [14], where the dynamic model is represented by a SDE and the measurement model is discrete in nature. We note that for these dynamic models, an exact discretization is not feasible, as is possible for some kinematic models such as the nearly constant velocity (NCV) and nearly constant acceleration (NCA) models [9], [4]. The CDF problem also arises from kinematic models in bearing-only tracking in 2D using using modified polar coordinates (MPC) [13], [1] or log polar coordinates (LPC) [6], [18] and bearing-only tracking in 3D using using modified spherical coordinates (MSC) [32], [2], [16], [21] or log spherical coordinates (LSC) [21]. Initially, SDEs were used for MPC [13] and MSC [32]. Then closed form analytic expressions for the exact discrete-time dynamic models for MPC and MSC were presented in [1] and [19], respectively. Alternatively, the discrete-time dynamic model for MSC can also be derived by a sequence of transformations [2], [22]. We observe that, the

1087

time evolution function for the continuous-discrete angle-only filtering problem in 3D using MSC is a nonlinear function of the target state. The discretization of the continuous-time stochastic dynamic model in many papers [23], [8], is unsatisfactory. We consider a general class of continuous-time stochastic dynamic models where the time evolution function is a nonlinear function of the target state and the process noise is dependent on the target state. This case arises for the angle-only filtering problem in 3D using MSC. This complex nonlinear dynamic model corresponds to the nearly constant velocity model (NCVM) in 3D using Cartesian coordinates. The paper is organized as follows. Section II presents a general continuous-time dynamic model where the time evolution function is a nonlinear in the target state and the additive Gaussian process noise is dependent on the target state. Discretization of the continuous-time dynamic model using the first and second order stochastic Taylor series approximations is proposed. This reasonably well-known mathematical tool does not seem to have been applied to target tracking problems. We apply the approximate discretization to 3D angleonly filtering using MSC in Section III. Nonlinear filtering algorithms using the approximate discretization are described in Section IV. Numerical simulation and results are presented in Sections V and the conclusions are summarized in Section VI.

notation is adopted for other vectors and matrices. Eq. (2) can be re-written in component-wise form as, for i = 1, . . . , d, Z t m Z t X xi (t) = xi (τ )+ fi (x(u)) du+ Gi,j (u) dβj (u). (3) τ

τ

+

m Z X j=1

t

Lj c(x(u), u) dβj (u),

L0 =

d m X d X X ∂ ∂ ∂ + + 1/2 , (5) fi Gi,l Gj,l ∂t i=1 ∂xi ∂x ∂xj i i,j=1 l=1

Lj =

d X

Gi,j

i=1

∂ . ∂xi

(6)

Applying (4) with c(·) = fi (·) and c(·) = Gi,j (·) to (3) for i = 1, . . . , d, j = 1, . . . , m, gives, after some manipulations, Z t m X Gi,j (x(τ )) xi (t) = xi (τ ) + (t − τ )fi (x(τ )) + dβj (u) j=1

Z tZ

+

L0 fi (x(r)) dr du

τ τ m Z t X j=1

+ +

u

Z

τ

Lj fi (x(r)) dβj (r) du

τ

m Z tZ X

u

L0 Gi,j (x(r)) dr dβj (u)

τ j=1 τ m Z tZ u X j,a=1

τ

u

+

(1)

where β(·) is a m-dimensional vector of independent, unit intensity Wiener processes and f (·) and G(·) are, in general, nonlinear functions. S¨arkk¨a considers a special case of (1) in [29], [30], where the process noise is independent of the target state. When measurements are obtained at discrete time-instants, the posterior density is propagated from one sampling instant to the next by solving the Fokker-Planck equation [14], [26]. Alternatively, we can solve (1) to obtain a discrete-time dynamical model and then apply the Chapman-Kolmogorov equation [14]. An exact implementation of this approach is possible, for instance, if f (·) is linear and G(·) does not depend on the state. More generally, discretization must be performed approximately. The discretization approach proposed here is based on truncated stochastic Taylor series expansions [17]. To introduce the stochastic Taylor series expansion we first re-write (1) in integral form as, for τ < t, Z t Z t x(t) = x(τ ) + f (x(u)) du + G(x(u)) dβ(u), (2)

(4)

τ

where

Consider a d-dimensional target state x(·). Many models of target motion can be described by a stochastic differential equation (SDE) of the form, for t ≥ 0,

τ

τ

Stochastic Taylor series expansions are obtained by applying the Itˆo lemma to (3). For a sufficiently smooth function c : Rnx × R → R, the Itˆo lemma states that Z t c(x(t), t) = c(x(τ ), τ ) + L0 c(x(u), u) du

II. D ISCRETIZATION OF DYNAMIC M ODELS

dx(t) = f (x(t)) dt + G(x(t)) dβ(t),

j=1

τ

La Gi,j (x(r)) dβj (r) dβa (u).

(7)

τ

The first-order stochastic Taylor series approximation, also called the Euler approximation, is obtained by ignoring all double integral terms in (7). This is equivalent to assuming that f (x(u)) ≈ f (x(τ )) and G(x(u)) ≈ G(x(τ )) for u ∈ [τ, t]. The Euler approximation is then given by Z t m X xi (t) ≈ xi (τ ) + (t − τ )fi (x(τ )) + Gi,j (x(τ )) dβj (u). j=1

τ

(8) Since

 Z

t

  τ    Z t 

 dβ1 (u)    ..  ∼ N (0, (t − τ )Im ), .   dβm (u)

τ

τ

where the second integral is an Itˆo integral [14]. Let xi (t) denote the ith element of the state vector x(t). A similar

where N (µ, Σ) denotes a Gaussian distribution with mean µ and covariance matrix Σ and Im is the m × m identity matrix,

1088

the Euler approximation to the transition kernel p(x(t)|x(τ )), t > τ ≥ 0, is pˆ1 (x(t)|x(τ )) = N (x(t); a1 (x(τ )), C1 (x(τ ))),

(9)

a1 (x) = x + (t − τ )f (x), C1 (x) = (t − τ )G(x)G(x)0 .

(10) (11)

where

The Euler approximation is usually simple to implement but can be inaccurate, particularly when applied over large intervals. An improved discretization can be obtained by using a high-order stochastic Taylor series approximation. A secondorder approximation is obtained by applying the Itˆo lemma to the terms c(·) = Lj fi (·) and c(·) = Lj Gi,l (·) in (7). This gives the approximation Z t m X xi (t) ≈ xi (τ ) + (t − τ )fi (x(τ )) + Gi,j (x(τ )) dβj (u) τ

j=1

+ L0 fi (x(τ ))

Z tZ

dr du τ

+ + +

m X j=1 m X

u

τ

Z tZ

j

u

L fi (x(τ ))

dβj (r) du τ

L0 Gi,j (x(τ ))

j=1 m X

τ

Z tZ

dr dβj (u) τ

La Gi,j (x(τ ))

τ

Z tZ

u

dβj (r) dβa (u). τ

j,a=1

u

(12)

τ

The second-order stochastic Taylor series approximation (12) is non-Gaussian except in the special case where La Gi,j (x) = 0 for i = 1, . . . , d, a, j = 1, . . . , m. In this special case the transition density p(x(t)|x(τ )) is approximated by pˆ2 (x(t)|x(τ )) = N (x(t); a2 (x(τ )), C2 (x(τ ))),

(13)

the case, there seems little point in considering higher-order approximations, particularly when arbitrary accuracy with a first-order or second-order approximation can be achieved by reducing the time interval over which it is applied. Thus the interval between measurements can be divided into subintervals with approximation (9) or (13) applied over successive sub-intervals. It can be expected that the second-order approximation will require fewer sub-intervals than the firstorder approximation to achieve a given level of accuracy. However, this is balanced by the higher computational expense of the second-order approximation so that better accuracy with a given computational expense may possibly be achieved by performing several first-order approximations rather than relatively few second-order approximations. III. A PPLICATION TO ANGLE - ONLY TRACKING A. Tracker Coordinate Frame (T frame) We define the tracker coordinate frame for which the X, Y , and Z axes are along the local east, north, and upward directions, respectively. The 3D angle-only tracking problem is considered under the following assumptions: 1) We estimate the state of a non-maneuvering target using bearing (β) and elevation () angle measurements. 2) The target motion is described by a NCVM in Cartesian coordinates [4] in 3D. The state of the target is defined in the T frame. 3) The motion of the ownship or sensor is deterministic, i.e., non-random. We know the state of the ownship 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 S frame is defined differently from that in [32]. B. Coordinate Systems for Target and Ownship

where a2 (x) = x + (t − τ )f (x) + (t − τ )2 L0 f (x)/2.

(14)

Let M(x) and N(x) be d × m matrices with typical element Mi,j (x) = Lj fi (x) and Ni,j (x) = L0 Gi,j (x). Define the d × 2m matrix   W(x) = M(x) − N(x) G(x) + (t − τ )N(x) . (15) The transition covariance matrix V2 (x) for the second-order stochastic Taylor series approximation is then given by 0

C2 (x) = W(x)(D ⊗ Im )W(x) , where

 D=

(t − τ )3 /3 (t − τ )2 /2 (t − τ )2 /2 t−τ

(16)

 .

(17)

Higher-order stochastic Taylor series approximations can be obtained by further applications of the Itˆo lemma. However such approximations become increasingly complicated as the order is increased. This is clear from a comparison of the firstorder (9) and second-order (13) approximations. This being

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 , (18) and xo :=



xo

yo

zo

x˙ o

y˙ o

z˙ o

0

.

(19)

The relative state vector of the target in the T frame is defined by x := xt − xo . (20) Let rT denote the range vector of the the target from the 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 . (21) The range is defined by p (22) r := krT k = x2 + y 2 + z 2 , r > 0.

1089

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

(25)

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

(26)

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

(27)

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

(28)

The relative state vector of the target in MSC is defined by [32]  0 ξ(t) := ξ1 (t) ξ2 (t) ξ3 (t) ξ4 (t) ξ5 (t) ξ6 (t) i0 h 1 ˙ ˙ ζ(t) β(t) (t) r(t) . (29) = ω(t) (t)

with  F(∆) =  Q(∆) =

where wk−1 ∼ N (· ; 0, Q(∆k )) and Z tk uk−1 = F(tk − t)BaT o (t) dt,

⊗ I3 , ∆2 /2 ∆

(35)  ⊗ diag(qx , qy , qz ).

(36)

1) Derive expressions for the relative velocity and relative acceleration as functions of range, bearing and elevation and their derivatives in the T frame. 2) Define a sensor frame (S frame) such that the Z axis of the S frame is along the range vector. 3) Using the bearing and elevation angles, calculate the rotational transformation matrix TST from the T frame to the S frame. 4) Transform the relative acceleration ¨rT in the T frame to the S frame to yield ¨rS which are functions of MSC. 5) Equate ¨rS to the difference of the target acceleration (white noise acceleration) and ownship acceleration, expressed in the S frame. This produces the desired stochastic differential equations for the MSC.

The dynamics for the relative state vector in Cartesian coordinates are described by (30)

The parameters qx , qy and qz are referred to as the process noise intensities. Let ∆k = tk − tk−1 denote the time between successive measurement sampling instants. The resulting discrete-time dynamic equation for the relative Cartesian state is xk = F(∆k )xk−1 + wk−1 − uk−1 , (33)

∆3 /3 ∆2 /2



It is desired to construct a discrete-time dynamical model for the relative state vector in MSC which is equivalent to the model (33) obtained for the relative state vector in Cartesian coordinates. We do this by writing the SDE (30) in MSC and then constructing truncated stochastic Taylor series approximations, as described in Section II. Note that it is possible in this particular problem to use the discretetime model (33) along with transformations between MSC and Cartesian coordinates to obtain an exact dynamical model in MSC. This approach is not always possible. The aim of this paper is to examine the performance of a general technique, hence the focus on the approximate discretization of Section II. We follow the approach of Stallard [32] to derive the SDE for MSC. The key steps of the derivation are as follows:

C. Dynamic Model for Relative State Vector in Cartesian Coordinates

where β(·) is a three-dimensional vector of independent, unit √ √ √ intensity Wiener processes, S = diag( qx , qy , qy ), aT o (·) is the sensor acceleration and   03 I3 A= , (31) 03 03   03 B= . (32) I3

∆ 1

D. Dynamic Model for Relative State Vector in Modified Spherical Coordinates

The components of the MSC defined in [32] and [21] are the same; however, the ordering is different.

dx(t) = Ax(t) dt + B (S dβ(t) − aT o (t) dt),

1 0

This approach yields the following SDE for the target state in MSC, for t ≥ 0, dξ(t) = f (ξ(t), t) dt + G(ξ(t))S dβ(t),

(37)

where

(34)

tk−1

1090

f (ξ, t) = s(ξ) − G(ξ)aT o (t),  cos ξ4  − sin ξ4 sin ξ5 G(ξ) = ξ6   sin ξ4 cos ξ5

− sin ξ4 − cos ξ4 sin ξ5 cos ξ4 cos ξ5 03

(38)  0 cos ξ5  , sin ξ5  (39)

IV. N ONLINEAR F ILTERING WITH A PPROXIMATE D ISCRETE - TIME DYNAMIC M ODEL USING MSC

with 

ξ1 (ξ2 tan ξ5 − 2ξ3 )  −2ξ2 ξ3 − ξ12 tan ξ5   ξ12 + ξ22 − ξ32 s(ξ) =   ξ1 / cos ξ5   ξ2 −ξ3 ξ6

    .   

(40)

Approximate discretization of (37) is performed below using the first-order and second-order truncated stochastic Taylor series expansions described in Section II. Let ξ k = ξ(tk ) denote the relative state vector in MSC at tk . A first order stochastic Taylor series approximation, or Euler approximation, results in the following stochastic difference equation for the time-evolution of the target state in MSC: ξ k ≈ a1 (ξ k−1 , tk−1 ; ∆k ) + vk ,

Nonlinear filters based on MSC are given in Algorithms 14. Algorithm 1 is the EKF [4], [9], Algorithm 2 is the UKF [15], Algorithm 3 is a PF [11], [3], [27] implemented with the prior as the importance density and Algorithm 4 is a PF implemented with the optimal importance density (OID) [3]. The filters make use of the jth order stochastic Taylor series approximation, j = 1, 2 applied over m intervals per sampling interval, m = 1, 2, . . .. Algorithm 1: A recursion of the EKF with MSC using approximate dynamic model ˆ 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 ˆ0 ˆ set ξ k|k−1 = ξ k−1|k−1 and Pk|k−1 = Pk−1|k−1 . for i = 1, . . . , m do compute the Jacobian Aij = Dξ0 aj (ξ, tk−1 + (i − 1)∆k /m; ∆k /m)|ξ=ξˆi−1 .

(41)

where vk ∼ N (· ; 0, C1 (ξ k−1 ; ∆k )) and a1 (ξ, t; ∆) = ξ + ∆f (ξ, t), C1 (ξ; ∆) = ∆G(ξ) diag(qx , qy , qz ) G(ξ)0 .

k|k−1

(42) (43)

ˆi compute the predicted mean ξ k|k−1 and covariance matrix i Pk|k−1 at time tk ˆi ˆi−1 ξ k|k−1 = aj (ξ k|k−1 , tk−1 + (i − 1)∆k /m; ∆k /m),

A more accurate approximation can be obtained using a second-order stochastic Taylor series expansion. Define the derivative of the matrix-valued function A with respect to the matrix-valued argument B as [20] DB A(B) = A(B) ⊗ ∇B ,

i 0 Pik|k−1 = Aij Pi−1 k|k−1 (Aj )

ˆi−1 , tk−1 + (i − 1)∆k /m; ∆k /m). + C j (ξ k|k−1 end 0 m compute the innovation covariance Sm k = HPk|k−1 H + R. m m 0 m −1 compute the gain matrix Kk = Pk|k−1 H (Sk ) . ˆ compute the posterior mean ξ k|k and covariance matrix Pk|k at time tk

(44)

where ∇B = [∂/∂bi,j ] with B = [bi,j ]. Also, denote the lth column of G as gl . Then, the second-order stochastic Taylor series approximation of the SDE (37) is ξ k ≈ a2 (ξ k−1 , tk−1 ; ∆k ) + vk ,

m ˆ ˆm ˆm ξ k|k = ξ k|k−1 + Kk (zk − Hξ k|k−1 ), m m m 0 Pk|k = Pm k|k−1 − Kk Sk (Kk ) .

(45)

where vk ∼ N (· ; 0, C2 (ξ k−1 , tk−1 ; ∆k )) and a2 (ξ, t; ∆) = ξ + ∆f (ξ, t) + ∆2 j(ξ, t)/2, (46)   C2 (ξ, t; ∆) = M(ξ, t) − N(ξ, t) G(ξ) + ∆N(ξ, t)   M(ξ, t)0 − N(ξ, t)0 × Q(∆) , (47) G(ξ)0 + ∆N(ξ, t)0 with j(ξ, t) = Dξ0 f (ξ, t) +

3 X (I6 ⊗ gl (ξ)0 )Dξ Dξ0 f (ξ, t), (48) l=1

M(ξ, t) = Dξ0 f (ξ, t)G(ξ), N(ξ, t) = Dξ0 G(ξ)(I3 ⊗ f (ξ, t)) +

(49)

3 X (I6 ⊗ gl (ξ)0 )Dξ Dξ0 G(ξ)(I3 ⊗ gl (ξ)). (50) l=1

The covariance matrix Q(∆) is given in (36).

V. N UMERICAL S IMULATIONS AND R ESULTS The scenario used in our simulation is similar to that used in [5]. 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 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 process noise intensities (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, as

1091

Algorithm 2: A recursion of the UKF with MSC using approximate dynamic model.

Algorithm 3: A recursion of the bootstrap filter in MSC with approximate dynamic model. 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 c(i) set ξik,0 = ξk−1 . for d = 0, . . . , m − 2 do draw ξik,d+1 ∼ N (· ; aj (ξik,d , tk−1 + d∆k /m; ∆k /m), Cj (ξik,d , tk−1 + d∆k /m; ∆k /m)). end draw ξik ∼ N (· ; aj (ξik,m−1 , tk − ∆k /m; ∆k /m), Cj (ξik,m−1 , tk − ∆k /m; ∆k /m) + Ωk , compute the un-normalized weight w ˜ki = N (zk ; Hξik , R). end compute the normalized weights , n X j i i w ˜k , i = 1, . . . , n. wk = w ˜k

ˆ 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 0 ˆ0 ˆ set ξ k|k−1 = ξ k−1|k−1 and Pk|k−1 = Pk−1|k−1 . for i = 1, . . . , m do construct matrix h the q i q Σi = 0 . (6 + κ)Pi−1 − (6 + κ)Pi−1 k|k−1 k|k−1 for b = 0, . . . , 12 do ˆi−1 + σ b where compute the sigma point Ξik,b = ξ k|k−1 σ b be the (b + 1)th column of Σi . compute the transformed sigma point and covariance matrix: Eik,b = aj (Ξik,b , tk−1 + (i − 1)∆k /m; ∆k /m), Cik,b = Cj (Ξik,b , tk−1 + (i − 1)∆k /m; ∆k /m). end ˆi compute the predicted mean ξ k|k−1 and covariance matrix i Pk|k−1 at time tk ˆi ξ k|k−1 = Pik|k−1

=

12 X b=0 12 X

j=1

wb Eik,b , wb [Cik,b

ˆ k at time tk compute the state estimate x

+

(Eik,b

−

i ˆi ξ k|k−1 )(Ek,b

−

ˆk = x

0 ˆi ξ k|k−1 ) ].

n X

C wki fMSC (ξik ).

i=1

b=0

end 0 m compute the innovation covariance Sm k = HPk|k−1 H + R. 0 m −1 m m compute the gain matrix Kk = Pk|k−1 H (Sk ) . ˆ compute the posterior mean ξ k|k and covariance matrix Pk|k at time tk

4

5.5

4.5

−

4

m m 0 Km k Sk (Kk ) .

3.5

Y (m)

Pk|k =

Ownship Trajectory

5

m ˆ ˆm ˆm ξ k|k = ξ k|k−1 + Kk (zk − Hξ k|k−1 ),

Pm k|k−1

x 10

3 2.5 2

illustrated in Figure 1.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 [22] with the parameters shown in Table I. All PFs are implemented with a sample size of 10,000. All filters use the first-order stochastic Taylor series approximation, i.e., the Euler approximation, with m = 8 discretization intervals per sampling interval.

1.5 1 0.5 0 −2

−1

0

X (m) Figure 1.

1

2 4

x 10

Various segments of the ownship trajectory.

The EKF is also implemented with Cartesian coordinates, referred to as the CEKF, to provide a baseline for the filters implemented with MSC. Two measures of performance are used to compare the algorithms described in Section IV. 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

1092

Table II M OTION PROFILE OF OWNSHIP FOR VARIOUS SEGMENTS .

Algorithm 4: A recursion of the OID PF in MSC with approximate dynamic model.

Time Interval (s) [[0, 15] [15, 31] [31, 43] [43, 75] [75, 86] [86, 102] [102, 210]

i {wk−1 , ξik−1 }

Input: a weighted sample from the posterior at time tk−1 and the measurement zk . Output: a weighted sample {wki , ξik } from the posterior at time tk . for i = 1, . . . , n do set ξik,0 = ξik−1 . for d = 0, . . . , m − 2 do draw ξik,d+1 ∼ N (· ; aj (ξik,d , tk−1 + d∆k /m; ∆k /m), Cj (ξik,d , tk−1 + d∆k /m; ∆k /m)). end compute the prior statistics ξik|k−1 = aj (ξik,m−1 , tk − ∆k /m; ∆k /m),

x 10

Motion Type CV CT CV CT CV CT CV

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

Target Ownship

9

Cik = Cj (ξik,m−1 , tk − ∆k /m; ∆k /m) + Ωk .

8 7

compute the prior measurement statistics zik = Hξik|k−1 and Sik = HCik H0 + R. compute the un-normalized weight i ψ˜ki = wk−1 N (zk ; zik , Sik ). end compute the normalized weights , n X j i i ˜ ψk = ψk ψ˜k ,

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

Target and Ownship Trajectories

4

10

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

Y (m)

6 5 4 3 2

i = 1, . . . , n.

j=1

1

select indices c(1), . . . , c(n) according to the weights ψk1 , . . . , ψkn . for i = 1, . . . , n do c(i) c(i) compute the gain matrix Ki = Ck H0 (Sk )−1 . i compute the posterior mean ξk|k and covariance matrix Σik at time tk c(i)

0

0

2

4

6

8

10

X (m) Figure 2.

4

x 10

Target and ownship trajectories.

c(i)

ξik|k = ξk|k−1 + Ki (zk − zk ),

noise standard deviations of 15 mrad and 35 mrad and slightly worse for a standard deviation of 1 mrad. The latter result is contrary to the usual case in which the use of MSC provides better performance than Cartesian coordinates. This suggests that discretization errors in the dynamical model become more significant as the measurement noise standard deviation decreases. The BF-MSC and OIDPF-MSC have much lower

− Ki Sk (Ki )0 .

c(i)

c(i)

Σik = Ck

draw ξik ∼ N (· ; ξik|k , Σik ) and set wki = 1/n. end ˆ k at time tk compute the state estimate x ˆk = x

n X

C wki fMSC (ξik ).

i=1

Table III T IME - AVERAGED RMS POSITION ERROR IN METERS .

Table I I NITIAL PARAMETERS OF TARGET. Variable ρ1 (km) β1 (deg) 1 (deg) z1t (km) s1 (m/s) c1 (deg) z˙1t (m/s)

Algorithm

Value 138.0 45.0 -0.415 9.0 297.0 -135.0 0.0

CEKF EKF-MSC UKF-MSC BF-MSC OIDPF-MSC

sdv (mrad) 1.0 0.650 0.732 0.735 3.728 3.710

sdv (mrad) 15.0 5.786 4.954 5.011 7.804 7.158

sdv (mrad) 35.0 12.00 10.81 10.66 13.53 11.38

Table IV F INAL RMS Algorithm

and IV. The execution times of the algorithms are given in Table V. Results in Tables III and IV show that the EKF-MSC and UKF-MSC have comparable state estimation accuracy. They perform significantly better than the CEKF for measurement

1093

CEKF EKF-MSC UKF-MSC BF-MSC OIDPF-MSC

POSITION ERROR IN METERS .

sdv (mrad) 1.0 0.472 0.569 0.583 3.833 3.827

sdv (mrad) 15.0 3.363 2.549 2.679 5.462 4.820

sdv (mrad) 35.0 4.780 3.447 3.615 6.535 5.166

accuracy than the Gaussian approximations. The performances of the PFs could be improved by increasing the sample size but it should be noted that even with the sample size of 10 000 used here, the PFs have a much larger computational expense than the EKF-MSC and UKF-MSC. Table V CPU TIMES OF FILTERING ALGORITHMS FOR THE MEASUREMENT STANDARD DEVIATION OF 15 MRAD . Algorithm CEKF EKF-MSC UKF-MSC BF-MSC OIDPF-MSC

CPU (s) 0.036 1.32 1.14 26.86 23.03

Relative CPU 1.000 36.667 31.667 746.111 639.722

VI. C ONCLUSIONS We have presented a general continuous-time dynamic model where the time evolution function is a nonlinear in the target state and the additive Gaussian process noise is dependent on the target state. Discretization of the continuoustime dynamic model using the first and second order Taylor series approximations is described which is a new and useful feature of the paper, and is not available in existing papers. This approach can be easily extended to a number of realworld problems of interest such as the CDF problem associated with the ballistic projectile, ballistic missile, and satellite orbit estimation. Our comparative results show that the CDF algorithms based on the EKF and UKF have nearly the same state estimation accuracy and computational cost. On the contrary, the PF based algorithms have a much lower state estimation accuracy and much higher computational cost. Thus, EKF and UKF based CDF algorithms are algorithms of choice for the current problem considered. We expect this trend to be valid for other CDF problems. R EFERENCES [1] V. J. Aidala and S. E. Hammel, Utilization of modified polar coordinates for bearings-only tracking, IEEE Trans. on Automatic Control, Vol. AC28, No. 3, pp. 283–294, March 1983. [2] 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. [3] 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 [4] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and Navigation, John Wiley & Sons, 2001. [5] S. S. Blackman, R. J. Dempster, 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. [6] 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. [7] T. P. Das, Relativistic Quantum Mechanics of Electrons, Harper and Row, 1973. [8] A. Farina, B. Ristic, and D. Benvenuti, Tracking a ballistic target: comparison of several nonlinear filters, IEEE Trans. on Aerospace and Electronic Systems, Vol. AES-38, No. 3, pp. 854 – 867, July 2002. [9] A. Gelb, Editor, Applied Optimal Estimation, MIT Press, 1974.

[10] H. Goldstein, C. P. Poole, and J. L. Safko, Classical Mechanics, Third Edition, Addison Wesley, 2001. [11] 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. [12] D. Halliday, R. Resnick, and J. Walker, Fundamentals of Physics Extended, Wiley, 2010. [13] 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. [14] A. Jazwinski, Stochastic Processes and Filtering Theory, Academic Press, 1970. [15] 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. [16] R. Karlsson and F. Gustafsson, Range estimation using angle-only target tracking with particle filters, Proc. American Control Conference, pp. 3743 – 3748, 2001. [17] P. E. Kloeden and E. Platen, Numerical Solution of Stochastic Differential Equations, Springer Verlag, 1992. [18] B. F. La Scala, M. Mallick, and S. Arulampalam, Differential Geometry Measures of Nonlinearity for Filtering with Nonlinear Dynamic and Linear Measurement Models, Proc. of SPIE, Vol. 6699, 2007. [19] 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. [20] E. C. MacRae, Matrix derivatives with an application to an adaptive decision problem, In The Annals of Statistics, Vol. 2, No. 2, pp. 337-345, 1974. [21] 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. [22] 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). [23] R. K. Mehra , A comparison of several nonlinear filters for reentry vehicle tracking, IEEE Trans. on Automatic Control, Vol. AC-16, No. 4, pp. 307–319, August 1971. [24] V. C. Ravindra and Y. Bar-Shalom, and P. Willett, Projectile Identification and Impact Point Prediction, IEEE Trans. on Aerospace and Electronic Systems, Vol. AES-46, No. 4, pp. 2004 - 2021, October 2010. [25] R. Resnick, Introduction to Special Relativity, Wiley, 1968. [26] H. Risken, The Fokker-Planck Equation, Second Edition,Springer, 1996. [27] B. Ristic, S. Arulampalam, and N. Gordon, Beyond the Kalman Filter, Artech House, 2004. [28] M. V. Samii, R. Shanklin, T. Lee, M. Mallick, and R. Kuseski, Comparative Studies of Atmospheric Density Models Used for Earth Satellite Orbit Estimation, J. Guidance, vol. 7, no. 2, pp. 235–237, March-April, 1984. [29] S. S¨arkk¨a, On sequential Monte Carlo sampling of discretely observed stochastic differential equations, In Proceedings of NSSPW, September 2006. [30] S. S¨arkk¨a, On Unscented Kalman Filtering for State Estimation of Continuous-Time Nonlinear Systems, IEEE Trans. on Automatic Control, Vol. AC-52, No. 9, pp. 1631–1641, September 2007. [31] R. Shankar, Principles of Quantum Mechanics, Second Edition, Springer, 1994. [32] 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, August 1987. [33] B. D. Tapley, B. E. Schutz, and G. H. Born, Statistical Orbit Determination, Elsevier Academic Press, 2004.

1094