Continuous-Discrete Filtering using EKF, UKF, and PF

Transcription

1 Continuous-Discrete Filtering using EKF, UKF, and PF Mahra Mallic 1, Mar Morelande 2, Lyudmila Mihaylova 3 1 Propagation Research Associates, Inc., Marietta, GA 30066, USA, mahra.mallic@gmail.com 2 The University of Melbourne, Parville VIC 3010, Australia, m.morelande@gmail.com 3 School of Computing and Communications, Lancaster University, United Kingdom, mila.mihaylova@lancaster.ac.u Abstract Continuous-discrete filtering (CDF) arises in many real-world problems such as ballistic projectile tracing, ballistic missile tracing, bearing-only tracing in 2D, angle-only tracing in 3D, and satellite orbit determination. We develop CDF algorithms using the exted Kalman filter (EKF), unscented Kalman filter (UKF), and particle filter (PF) with applications to the angle-only tracing 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 Exted Kalman filter, Continuous-discrete Unscented Kalman filter, Continuous-discrete Particle filter. I. INTRODUCTION 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ödinger equation in the non-relativistic domain [31] or Dirac equation in the relativistic domain [7]). In tracing applications, we usually deal with macroscopic objects in the non-relativistic domain and two types of equations of motion are used; inematic equations and dynamic equations. Equations for constant velocity (CV), constant acceleration(ca), and coordinated turn represent inematic 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 framewor is a robust and natural framewor, 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], 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 inematic models such as the nearly constant velocity (NCV) and nearly constant acceleration (NCA) models [9], [4]. The CDF problem also arises from inematic models in bearing-only tracing in 2D using using modified polar coordinates (MPC) [13], [1] or log polar coordinates (LPC) [6], [18] and bearing-only tracing 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

2 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 the time evolution function is a nonlinear function of the target state and the process noise is depent 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 the time evolution function is a nonlinear in the target state and the additive Gaussian process noise is depent 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-nown mathematical tool does not seem to have been applied to target tracing 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. II. DISCRETIZATION OF DYNAMIC MODELS 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, dx(t) = f(x(t)) dt G(x(t)) dβ(t), (1) β( ) is a m-dimensional vector of indepent, unit intensity Wiener processes and f( ) and G( ) are, in general, nonlinear functions. Särä considers a special case of (1) in [29], [30], the process noise is indepent 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 Foer-Planc 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 dep 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, x(t) = x() f(x(u)) du G(x(u)) dβ(u), (2) the second integral is an Itô integral [14]. Let x i (t) denote the ith element of the state vector x(t). A similar notation is adopted for other vectors and matrices. Eq. (2) can be re-written in component-wise form as, for i = 1,..., d, x i (t) = x i () f i (x(u)) du G i,j (u) dβ j (u). (3) Stochastic Taylor series expansions are obtained by applying the Itô lemma to (3). For a sufficiently smooth function c : R nx R R, the Itô lemma states that c(x(t), t) = c(x(), ) L 0 = t d L j = d i=1 i=1 G i,j f i 1/2 x i L 0 c(x(u), u) du L j c(x(u), u) dβ j (u), (4) l=1 i, d G i,l G j,l, (5) x i x j. (6) x i Applying (4) with c( ) = f i ( ) and c( ) = G i,j ( ) to (3) for i = 1,..., d, j = 1,..., m, gives, after some manipulations, x i (t) = x i () (t )f i (x()) G i,j (x()) dβ j (u) u u u j,a=1 L 0 f i (x(r)) dr du u L j f i (x(r)) dβ j (r) du L 0 G i,j (x(r)) dr dβ j (u) L a G i,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 x i (t) x i () (t )f i (x()) G i,j (x()) dβ j (u). Since dβ 1 (u). dβ m (u) N(0, (t )I m ), N(µ, Σ) denotes a Gaussian distribution with mean µ and covariance matrix Σ and I m is the m m identity matrix, (8) 1088

3 the Euler approximation to the transition ernel p(x(t) x()), t > 0, is ˆp 1 (x(t) x()) = N(x(t); a 1 (x()), C 1 (x())), (9) a 1 (x) = x (t )f(x), (10) C 1 (x) = (t )G(x)G(x). (11) 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ô lemma to the terms c( ) = L j f i ( ) and c( ) = L j G i,l ( ) in (7). This gives the approximation x i (t) x i () (t )f i (x()) G i,j (x()) dβ j (u) L 0 f i (x()) u L j f i (x()) L 0 G i,j (x()) dr du u L a G i,j (x()) j,a=1 u u dβ j (r) du dr dβ j (u) dβ j (r) dβ a (u). (12) The second-order stochastic Taylor series approximation (12) is non-gaussian except in the special case L a G i,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); a 2 (x()), C 2 (x())), (13) a 2 (x) = x (t )f(x) (t ) 2 L 0 f(x)/2. (14) Let M(x) and N(x) be d m matrices with typical element M i,j (x) = L j f i (x) and N i,j (x) = L 0 G i,j (x). Define the d 2m matrix W(x) = [ M(x) N(x) G(x) (t )N(x) ]. (15) The transition covariance matrix V 2 (x) for the second-order stochastic Taylor series approximation is then given by C 2 (x) = W(x)(D I m )W(x), (16) D = [ (t ) 3 /3 (t ) 2 /2 (t ) 2 /2 t ]. (17) Higher-order stochastic Taylor series approximations can be obtained by further applications of the Itô 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 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. APPLICATION TO ANGLE-ONLY TRACKING A. Tracer Coordinate Frame (T frame) We define the tracer coordinate frame for which the X, Y, and Z axes are along the local east, north, and upward directions, respectively. The 3D angle-only tracing 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 now 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 T S T from the T frame to the S frame is defined differently from that in [32]. B. Coordinate Systems for Target and Ownship 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 and x t := [ x t y t z t ẋ t ẏ t ż t ], (18) x o := [ x o y o z o ẋ o ẏ o ż o ]. (19) The relative state vector of the target in the T frame is defined by x := x t x o. (20) Let r T denote the range vector of the the target from the sensor in the T frame. Then r T is defined by r T := [ x y z ] [ = x t x o y t y o z t z o ]. (21) The range is defined by r := r T = x 2 y 2 z 2, r > 0. (22) 1089

4 The range vector can be expressed in terms of range, bearing (β) and elevation (ɛ) by cos ɛ sin β r T = r cos ɛ cos β, β [0, 2π], ɛ [ π/2, π/2]. sin ɛ (23) The ground range is defined by ρ := x 2 y 2 = r cos ɛ, ρ > 0. (24) Modified Spherical Coordinates for Relative State Vector: Following Stallard s convention [32], we use ω as a component of the MSC, ω(t) := β(t) cos ɛ(t). (25) Let ζ(t) denote the logarithm of range r(t) Then ζ(t) := ln r(t). (26) r(t) = exp[ζ(t)]. (27) Differentiating (26) with respect to time, we get ζ(t) = ṙ(t) r(t). (28) The relative state vector of the target in MSC is defined by [32] ξ(t) := [ ξ 1 (t) ξ 2 (t) ξ 3 (t) ξ 4 (t) ξ 5 (t) ξ 6 (t) ] [ ] = ω(t) ɛ(t) ζ(t) β(t) ɛ(t) 1 r(t). (29) The components of the MSC defined in [32] and [21] are the same; however, the ordering is different. C. Dynamic Model for Relative State Vector in Cartesian Coordinates The dynamics for the relative state vector in Cartesian coordinates are described by dx(t) = Ax(t) dt B (S dβ(t) a T o (t) dt), (30) β( ) is a three-dimensional vector of indepent, unit intensity Wiener processes, S = diag( q x, q y, q y ), a T o ( ) is the sensor acceleration and [ ] 03 I A = 3, (31) [ ] 03 B =. (32) I 3 The parameters q x, q y and q z are referred to as the process noise intensities. Let = t t 1 denote the time between successive measurement sampling instants. The resulting discrete-time dynamic equation for the relative Cartesian state is x = F( )x 1 w 1 u 1, (33) w 1 N( ; 0, Q( )) and u 1 = t 1 F(t t)ba T o (t) dt, (34) with [ ] 1 F( ) = I 0 1 3, (35) [ ] Q( ) = 3 /3 2 /2 2 diag(q /2 x, q y, q z ). (36) D. Dynamic Model for Relative State Vector in Modified Spherical Coordinates 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 ey steps of the derivation are as follows: 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 T S T from the T frame to the S frame. 4) Transform the relative acceleration r T in the T frame to the S frame to yield r S which are functions of MSC. 5) Equate r S 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. 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) f(ξ, t) = s(ξ) G(ξ)a T o (t), (38) cos ξ 4 sin ξ 4 0 G(ξ) = ξ 6 sin ξ 4 sin ξ 5 cos ξ 4 sin ξ 5 cos ξ 5 sin ξ 4 cos ξ 5 cos ξ 4 cos ξ 5 sin ξ 5, 0 3 (39) 1090

5 with s(ξ) = ξ 1 (ξ 2 tan ξ 5 2ξ 3 ) 2ξ 2 ξ 3 ξ 2 1 tan ξ 5 ξ 2 1 ξ 2 2 ξ 2 3 ξ 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 ξ = ξ(t ) denote the relative state vector in MSC at t. 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: ξ a 1 (ξ 1, t 1 ; ) v, (41) v N( ; 0, C 1 (ξ 1 ; )) and a 1 (ξ, t; ) = ξ f(ξ, t), (42) C 1 (ξ; ) = G(ξ) diag(q x, q y, q z ) G(ξ). (43) 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] D B A(B) = A(B) B, (44) B = [ / b i,j ] with B = [b i,j ]. Also, denote the lth column of G as g l. Then, the second-order stochastic Taylor series approximation of the SDE (37) is ξ a 2 (ξ 1, t 1 ; ) v, (45) IV. NONLINEAR FILTERING WITH APPROXIMATE DISCRETE-TIME DYNAMIC MODEL USING MSC Nonlinear filters based on MSC are given in Algorithms 1-4. 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 mae 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 ˆξ 1 1 and covariance matrix P 1 1 at time t 1 and the measurement z. Output: posterior mean ˆξ and covariance matrix P at time t. set ˆξ 0 1 = ˆξ 1 1 and P 0 1 = P 1 1. for i = 1,..., m do compute the Jacobian A i j = D ξ a j(ξ, t 1 (i 1) /m; /m). ξ=ˆξi 1 1 compute the predicted mean ˆξ i 1 and covariance matrix P i 1 at time t ˆξ i 1 = aj(ˆξ i 1 1, t 1 (i 1) /m; /m), P i 1 = A i jp i 1 1 (Ai j) C j(ˆξ i 1 1, t 1 (i 1) /m; /m). compute the innovation covariance S m = HP m 1H R. compute the gain matrix K m = P m 1H (S m ) 1. compute the posterior mean ˆξ and covariance matrix P at time t ˆξ = ˆξ m 1 Km (z Hˆξ m 1 ), P = P m 1 K m S m (K m ). v N( ; 0, C 2 (ξ 1, t 1 ; )) and a 2 (ξ, t; ) = ξ f(ξ, t) 2 j(ξ, t)/2, (46) C 2 (ξ, t; ) = [ M(ξ, t) N(ξ, t) G(ξ) N(ξ, t) ] [ ] M(ξ, t) Q( ) N(ξ, t) G(ξ) N(ξ, t), (47) with j(ξ, t) = D ξ f(ξ, t) 3 (I 6 g l (ξ) )D ξ D ξ f(ξ, t), (48) l=1 M(ξ, t) = D ξ f(ξ, t)g(ξ), (49) N(ξ, t) = D ξ G(ξ)(I 3 f(ξ, t)) 3 (I 6 g l (ξ) )D ξ D ξ G(ξ)(I 3 g l (ξ)). (50) l=1 The covariance matrix Q( ) is given in (36). V. NUMERICAL SIMULATIONS AND RESULTS The scenario used in our simulation is similar to that used in [5]. We have made some changes to mae the scenario three dimensional in nature. Initial height z o 1 of the ownship is 10.0 m. Target s initial ground range ρ 1, bearing β 1, and height z t 1 are shown in Table I. Then the initial elevation ɛ 1 can be calculated. Table I also shows target s initial speed s 1, course c 1 in the XY -plane and the Z component of velocity ż t 1. Target s initial position and velocity in Cartesian coordinates can be found from Table I as (138/ 2, 138/ 2, 9) m and 297/ 2( 1, 1, 0) m/s, respectively. The motion of the target is governed by the NCVM. The process noise intensities (q x, q y, q z ) of the zero-mean white acceleration process noise along the X, Y, and Z axes of the T frame are (0.01, 0.01, ) m 2 s 3, respectively. The ownship moves in a plane parallel to the XY -plane at a fixed height of 10 m with segments of CV and CT, as 1091

6 Algorithm 2: A recursion of the UKF with MSC using approximate dynamic model. Input: posterior mean ˆξ 1 1 and covariance matrix P 1 1 at time 1 and the measurement z Output: posterior mean ˆξ and covariance matrix P at time set ˆξ 0 1 = ˆξ 1 1 and P 0 1 = P 1 1. for i = 1,..., m do construct [ the matrix Σ i = 0 for b = 0,..., 12 do (6 κ)p i 1 1 (6 κ)p i 1 1 ]. compute the sigma point Ξ i i 1,b = ˆξ 1 σ b σ b be the (b 1)th column of Σ i. compute the transformed sigma point and covariance matrix: E i,b = a j(ξ i,b, t 1 (i 1) /m; /m), C i,b = C j(ξ i,b, t 1 (i 1) /m; /m). compute the predicted mean ˆξ i 1 and covariance matrix P i 1 at time t ˆξ i 1 = P i 1 = 12 b=0 12 b=0 w b E i,b, w b [C i,b (E i,b ˆξ i 1 )(Ei,b ˆξ i 1 ) ]. compute the innovation covariance S m = HP m 1H R. compute the gain matrix K m = P m 1H (S m ) 1. compute the posterior mean ˆξ and covariance matrix P at time t ˆξ = ˆξ m 1 K m (z Hˆξ m 1), P = P m 1 K m S m (K m ). Algorithm 3: A recursion of the bootstrap filter in MSC with approximate dynamic model. Input: a weighted sample {w 1, i ξ i 1 } from the posterior at time t 1 and the measurement z. Output: a weighted sample {w, i ξ i } from the posterior at time t. select indices c(1),..., c(n) according to the weights w 1, 1..., w 1. n for i = 1,..., n do set ξ i,0 = ξ c(i) 1. for d = 0,..., m 2 do draw ξ i,d1 N( ; a j(ξ i,d, t 1 d /m; /m), C j(ξ i,d, t 1 d /m; /m)). draw ξ i N( ; aj(ξi,m 1, t /m; /m), C j(ξ i,m 1, t /m; /m) Ω, compute the un-normalized weight w i = N(z ; Hξ i, R). compute the normalized weights / n w i = w i, i = 1,..., n. w j compute the state estimate ˆx at time t n ˆx = wf i MSC(ξ C i ). i=1 5.5 x 104 Ownship Trajectory Y (m) 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, 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. Figure X (m) x 10 4 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 of the last observer maneuver to the of the surveillance period. The second performance measure is the RMS position error at the of the surveillance period. The results are shown in Tables III 1092

7 Algorithm 4: A recursion of the OID PF in MSC with approximate dynamic model. Input: a weighted sample {w 1, i ξ i 1 } from the posterior at time t 1 and the measurement z. Output: a weighted sample {w, i ξ i } from the posterior at time t. for i = 1,..., n do set ξ i,0 = ξ i 1. for d = 0,..., m 2 do draw ξ i,d1 N( ; a j(ξ i,d, t 1 d /m; /m), C j(ξ i,d, t 1 d /m; /m)). compute the prior statistics ξ i 1 = a j(ξ i,m 1, t /m; /m), C i = C j(ξ i,m 1, t /m; /m) Ω. compute the prior measurement statistics z i = Hξ i 1 and S i = HC i H R. compute the un-normalized weight ψ i = w 1N(z i ; z i, S i ). compute the normalized weights / ψ i = ψ n i, i = 1,..., n. ψ j select indices c(1),..., c(n) according to the weights ψ, 1..., ψ n. for i = 1,..., n do compute the gain matrix K i = C c(i) H (S c(i) ) 1. compute the posterior mean ξ i and covariance matrix Σ i at time t ξ i = ξ c(i) 1 Ki (z z c(i) ), Σ i = C c(i) K i S c(i) (K i ). draw ξ i N( ; ξi, Σi ) and set w i = 1/n. compute the state estimate ˆx at time t n ˆx = wf i MSC(ξ C i ). i=1 Y (m) Table II MOTION PROFILE OF OWNSHIP FOR VARIOUS SEGMENTS. Time Interval t φ Motion Type ω o (s) (s) (rad) (rad/s) [[0, 15] 15 0 CV 0 [15, 31] 16 π/4 CT π/64 [31, 43] 12 0 CV 0 [43, 75] 32 π/2 CT π/64 [75, 86] 11 0 CV 0 [86, 102] 16 π/4 CT π/64 [102, 210] CV 0 10 x 104 Target and Ownship Trajectories Figure 2. Target Ownship X (m) Target and ownship trajectories. x 10 4 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 Table I INITIAL PARAMETERS OF TARGET. Variable Value ρ 1 (m) β 1 (deg) 45.0 ɛ 1 (deg) z1 t (m) 9.0 s 1 (m/s) c 1 (deg) ż1 t (m/s) 0.0 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 Table III TIME-AVERAGED RMS POSITION ERROR IN METERS. Algorithm sdv (mrad) sdv (mrad) sdv (mrad) CEKF EKF-MSC UKF-MSC BF-MSC OIDPF-MSC Table IV FINAL RMS POSITION ERROR IN METERS. Algorithm sdv (mrad) sdv (mrad) sdv (mrad) CEKF EKF-MSC UKF-MSC BF-MSC OIDPF-MSC

8 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 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 CPU (s) Relative CPU CEKF EKF-MSC UKF-MSC BF-MSC OIDPF-MSC VI. CONCLUSIONS We have presented a general continuous-time dynamic model the time evolution function is a nonlinear in the target state and the additive Gaussian process noise is depent 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 exted 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 tr to be valid for other CDF problems. REFERENCES [1] V. J. Aidala and S. E. Hammel, Utilization of modified polar coordinates for bearings-only tracing, IEEE Trans. on Automatic Control, Vol. AC- 28, No. 3, pp , March [2] R. R. Allen and S. S. Blacman, Implementation of an angle-only tracing filter, Proc. of SPIE, Signal and Data Processing of Small Targets, Vol. 1481, Orlando, FL, USA, April [3] M. Arulampalam, S. Masell, N. Gordon and T. Clapp, A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracing, IEEE Trans. on Signal Processing, Vol. SP-50, No. 2, pp , February 2002 [4] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with Applications to Tracing and Navigation, John Wiley & Sons, [5] S. S. Blacman, R. J. Dempster, B. Blyth, and C. Durand, Integration of Passive Ranging with Multiple Hypothesis Tracing (MHT) for Application with Angle-Only Measurements, Proc. of SPIE, Vol. 7698, pp , [6] T. Bréhard and J-P. Le Cadre, Closed-form posterior Cramér-Rao bounds for bearings only tracing, IEEE Trans. on Aerospace and Electronic Systems, Vol. AES-42, No. 4, pp , October [7] T. P. Das, Relativistic Quantum Mechanics of Electrons, Harper and Row, [8] A. Farina, B. Ristic, and D. Benvenuti, Tracing a ballistic target: comparison of several nonlinear filters, IEEE Trans. on Aerospace and Electronic Systems, Vol. AES-38, No. 3, pp , July [9] A. Gelb, Editor, Applied Optimal Estimation, MIT Press, [10] H. Goldstein, C. P. Poole, and J. L. Safo, Classical Mechanics, Third Edition, Addison Wesley, [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 , April [12] D. Halliday, R. Resnic, and J. Waler, Fundamentals of Physics Exted, Wiley, [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, [14] A. Jazwinsi, Stochastic Processes and Filtering Theory, Academic Press, [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 , March [16] R. Karlsson and F. Gustafsson, Range estimation using angle-only target tracing with particle filters, Proc. American Control Conference, pp , [17] P. E. Kloeden and E. Platen, Numerical Solution of Stochastic Differential Equations, Springer Verlag, [18] B. F. La Scala, M. Mallic, and S. Arulampalam, Differential Geometry Measures of Nonlinearity for Filtering with Nonlinear Dynamic and Linear Measurement Models, Proc. of SPIE, Vol. 6699, [19] Q. Li, F. Guo, Y. Zhou, and W. Jiang, Observability of Satellite to Satellite Passive Tracing from Angles Measurements, Proc. IEEE International Conference on Control and Automation, pp , [20] E. C. MacRae, Matrix derivatives with an application to an adaptive decision problem, In The Annals of Statistics, Vol. 2, No. 2, pp , [21] M. Mallic, 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, [22] M. Mallic, M. Morelande, L. Mihaylova, S. Arulampalam, and Y. Yan, Angle-only Filtering in 3D, Chapter 1, In Integrated Tracing, Classification, and Sensor Management: Theory and Applications, M. Mallic, 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 tracing, IEEE Trans. on Automatic Control, Vol. AC-16, No. 4, pp , August [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 , October [25] R. Resnic, Introduction to Special Relativity, Wiley, [26] H. Risen, The Foer-Planc Equation, Second Edition,Springer, [27] B. Ristic, S. Arulampalam, and N. Gordon, Beyond the Kalman Filter, Artech House, [28] M. V. Samii, R. Shanlin, T. Lee, M. Mallic, and R. Kusesi, Comparative Studies of Atmospheric Density Models Used for Earth Satellite Orbit Estimation, J. Guidance, vol. 7, no. 2, pp , March-April, [29] S. Särä, On sequential Monte Carlo sampling of discretely observed stochastic differential equations, In Proceedings of NSSPW, September [30] S. Särä, On Unscented Kalman Filtering for State Estimation of Continuous-Time Nonlinear Systems, IEEE Trans. on Automatic Control, Vol. AC-52, No. 9, pp , September [31] R. Shanar, Principles of Quantum Mechanics, Second Edition, Springer, [32] D. V. Stallard, An angle-only tracing filter in modified spherical coordinates, Proc. of the AIAA Guidance, Navigation and Control Conference, Monterey, CA, pp , August [33] B. D. Tapley, B. E. Schutz, and G. H. Born, Statistical Orbit Determination, Elsevier Academic Press,