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 Mallick 1 , Mark Morelande 2 , Lyudmila Mihaylova 3 , Sanjeev Arulampalam 4 , and Yanjun Yan 5
1 Propagation Research Associates, Inc., Marietta, GA 30066, USA, mahendra.mallick@gmail.com
2 The University of Melbourne, Parkville VIC 3010, Australia, m.morelande@gmail.com
3 School of Computing and Communications, Lancaster University, United Kingdom, mila.mihaylova@lancaster.ac.uk
4 Maritime Operations Division, DSTO, Edinburgh SA 5111, Australia,
sanjeev.arulampalam@dsto.defence.gov.au
5 ARCON Corporation, Waltham, MA, 02452, USA, yanjun@arcon.com
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. INTRODUCTION
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 t k−1 . First, the MSC are

transformed to relative Cartesian coordinates at time t k−1 .
Then the relative Cartesian coordinates at time t k are obtained
by using the NCVM for relative Cartesian coordinates during
the time interval [t k−1 , t k ]. Finally, the relative Cartesian coordinates
are transformed to MSC at time t k . 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 [t k−1 , t k ]. 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. COORDINATE SYSTEMS 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 h 0 . 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 T S T from the T frame
to the sensor frame (S frame) is defined differently in our
approach compared with that in [36].
Local
East
X T
Z T
Ownship
x
Local
Up
ε
β
Target
z
y
Y T
Local
North
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
and
x t := [ x t y t z t ẋ t ẏ t ż t ] ′
, (1)
x o := [ x o y o z o ẋ o ẏ o ż o ] ′
. (2)
The relative state vector in the T frame is defined by
x := x t − x o . (3)
Let x = [x y z ẋ ẏ ż] ′ denote the relative state vector
in the T frame. Then x = x t − x o , ẋ = ẋ t − ẋ o , etc. Let r T
denote the range vector of the target from the ownship (or
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 ] ′
.
(4)

The range is defined by
r := ‖r T ‖ = √ x 2 + y 2 + z 2 , r > 0. (5)
The range vector can be expressed in terms of range, bearing
(β) and elevation (ɛ), as defined in Figure 1, by
⎡
cos ɛ sin β
⎤
r T = r ⎣ cos ɛ cos β
sin ɛ
⎦ , β ∈ [0, 2π], ɛ ∈ [−π/2, π/2].
(6)
The ground range is defined by
ρ := √ x 2 + y 2 = r cos ɛ, ρ > 0. (7)
B. Modified Spherical Coordinates for Relative State Vector
Following Stallard’s convention [36], we use ω as a component
of the MSC, where
ω(t) := ˙β(t) cos ɛ(t). (8)
Let ζ(t) denote the logarithm of range r(t)
Then
ζ(t) := ln r(t). (9)
r(t) = exp[ζ(t)]. (10)
Differentiating (9) with respect to time, we get
˙ζ(t) = ṙ(t)/r(t). (11)
The relative state vector of the target in MSC is defined by
[28], [36]
ξ(t) := [ ξ 1 (t) ξ 2 (t) ξ 3 (t) ξ 4 (t) ξ 5 (t) ξ 6 (t) ] ′
= [ ω(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 MODELS
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 x t k
denote the Cartesian state of the target at time
t k . Then the discrete-time dynamic model of the target is
described by [8], [14]
x t k = F k−1 x t k−1 + w t k−1, (13)
where F k−1 and wk−1 t are the state transition matrix [8], [14]
and integrated process noise [14], respectively, for the time
interval [t k−1 , t k ],
[ ] 1 ∆k
F k−1 = F(t k , t k−1 ) :=
⊗ I
0 1 3 , (14)
w t k−1 :=
∫ tk
t k−1
F(t k − t)w t (t) dt, (15)
where ∆ k := t k − t k−1 . In (14), ⊗ refers to the Kronecker
product [19]. Using the properties of the continuous-time
process noise w t (t), state transition matrix F k−1 defined in
(14), and the definition of wk−1 t in (15), we can show that
wk−1 t is a zero-mean Gaussian white integrated process noise
with covariance Q k−1
wk−1 t ∼ N(wk−1; t 0, Q k−1 ), (16)
[ ∆
3
Q k−1 = k
/3 ∆ 2 k /2 ]
∆ 2 k /2 ⊗ diag(q x , q y , q z ), (17)
∆ k
where q x , q y , q z 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
where
x o k = F k−1 x o k−1, (18)
x o k = F CT (∆ k , ω)x o k−1, (19)
F CT (∆, ω) =
⎡
⎤
1 0 0 sin(ω∆)/ω −[1 − cos(ω∆)]/ω 0
0 1 0 [1 − cos(ω∆)]/ω sin(ω∆)/ω 0
0 0 1 0 0 ∆
⎢ 0 0 0 cos(ω∆) − sin(ω∆) 0
.
⎥
⎣ 0 0 0 sin(ω∆) cos(ω∆) 0 ⎦
0 0 0 0 0 1
(20)
Using (13) in the definition (3) of the relative state vector
and then adding and subtracting F k−1 x o k−1
, we get
where
x k = F k−1 x k−1 + w t k−1 − u k−1 , (21)
u k−1 := x o k − F k−1 x o k−1. (22)
We note that when ω o k approaches zero, FCT (∆ k , ω o k ) reduces
to F k−1 and u k−1 becomes zero.
B. Dynamic Model for Relative State Vector in Modified
Spherical Coordinates
Let f MSC
C : R 6 → R 6 denote the transformation from
relative Cartesian coordinates to MSC. Similarly, let f C MSC :
R 6 → R 6 denote the inverse transformation from MSC to
relative Cartesian coordinates. Then,
ξ k = f MSC
C (x k ), (23)
x k = f C MSC(ξ k ). (24)
Functional forms of fC
MSC (x k ) and fMSC C (ξ k) are presented in
[29]. Then, using (21) in (23), we get
ξ k = f MSC
C (F k−1 x k−1 + w t k−1 − u k−1 ). (25)

We have
Substitution of (26) in (25) gives
x k−1 = f C MSC(ξ k−1 ). (26)
ξ k = f MSC
C (F k−1 f C MSC(ξ k−1 ) + w t k−1 − u k−1 ). (27)
Formally, we can write (27) as
ξ k = b(ξ k−1 , u k−1 , w t k−1), (28)
where b is a nonlinear function of ξ k−1 , u k−1 , and wk−1 t . 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
seen from (28) that the process noise wk−1
t 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.
IV. MEASUREMENT MODELS
The passive sensor collects bearing and elevation measurements
{z k } at discrete times {t k }. The measurement model for
the bearing and elevation angles using the relative Cartesian
state vector x k is
where
h(x k ) :=
z k = h(x k ) + n k , (29)
[
βk
]
=
ɛ k
[ tan −1 (x k , y k )
tan −1 (z k , ρ k )
]
, (30)
where the ground range ρ k is defined in (7) and n k is a zeromean
white Gaussian measurement noise with covariance R
n k ∼ N (n k ; 0, R), R := diag(σ 2 β, σ 2 ɛ ). (31)
The measurement model for the bearing and elevation
angles β k , ɛ k using MSC is
H :=
z k = Hξ k + n k , (32)
[ ]
0 0 0 1 0 0
.
0 0 0 0 1 0
(33)
V. FILTER INITIALIZATION
We initialize the filter using the first measurement z 1 at
t 1 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, α, γ] ′ where s is speed, α, and
γ are the bearing and elevation of target velocity. Let φ 1
denote the vector φ at time t 1 . Let ¯r, ¯s, ᾱ, and ¯γ denote the
prior mean for range, speed, α and γ, respectively. Similarly,
let σr, 2 σs, 2 σα, 2 and σγ
2 denote the prior variances for range,
speed, α and γ, respectively. Then the distribution of φ 1 given
the measurement z 1 and prior information on the range and
velocity of the target is
φ 1 |z 1 ∼ N(· ; ˆφ 1 , Σ 1 ), (34)
where
ˆφ 1 = [ z ′ 1 ¯r ¯s ᾱ ¯γ ] ′
, (35)
Σ 1 = diag(σ 2 β, σ 2 ɛ , σ 2 r, σ 2 s, σ 2 α, σ 2 γ). (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
∫
ˆx 1 = u(φ)N(φ; ˆφ 1 , Σ 1 ) dφ, (37)
∫
P 1 = [u(φ) − ˆx 1 ][u(φ) − ˆx 1 ] ′ N(φ; ˆφ 1 , Σ 1 ) dφ, (38)
where
⎡
u(φ) =
⎢
⎣
r cos ɛ sin β
r cos ɛ cos β
r sin ɛ
s cos γ sin α − ẋ o 1
s cos γ cos α − ẏ o 1
s sin γ − ż o 1
⎤
. (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 x i 1 = u(φ i ) where φ i ∼ N(· ; ˆφ 1 , Σ 1 ) for i =
1, . . . , n. The sample weights are w i 1 = 1/n, i = 1, . . . , n.
B. Initialization of Modified Spherical Coordinates
The first three components of the MSC are given by
ξ 1 = ω = (yẋ − xẏ)/ρr, (40)
ξ 2 = ˙ɛ = (żρ 2 − z(xẋ + yẏ)/ρr 2 , (41)
ξ 3 = ˙ζ = (xẋ + yẏ + zż)/r 2 . (42)
Using (4)-(7) in (40)-(42), we get
⎡ ⎤
⎡
ξ 1
⎣ ξ 2
⎦ = 1 r A(β, ɛ) ⎣ ẋ
ẏ
ξ 3
ż
where
⎡
A(β, ɛ) := ⎣
cos β − sin β 0
− sin β sin ɛ − cos β sin ɛ cos ɛ
sin β cos ɛ cos β cos ɛ sin ɛ
⎤
⎦ , (43)
⎤
⎦ . (44)
The mean and covariance matrix of the relative target state in
MSC given a vector φ distributed according to (34) are
∫
ˆξ 1 = v(φ)N(φ; ˆφ 1 , Σ 1 ) dφ, (45)
∫
P 1 = [v(φ) − ˆξ 1 ][v(φ) − ˆξ 1 ] ′ N(φ; ˆφ 1 , Σ 1 ) dφ, (46)

where
⎡
v(φ) =
⎢
⎣
0
0
0
β
ɛ
1/r
⎤
+ 1
⎥ r
⎦
[ A(β, ɛ)
0 3
] ⎡ ⎣
s cos γ sin α − ẋ o 1
s cos γ cos α − ẏ o 1
s sin γ − ż o 1
(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 P 1 are not presented in the paper but can be found in
[29].
VI. NONLINEAR FILTERING USING RELATIVE CARTESIAN
COORDINATES
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
⎤
⎦ .
VII. NONLINEAR FILTERING USING MSC
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
both the previous state ξ k−1 and the process noise w t k−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 w 0 =
κ/(12 + κ) and w i = 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
P k−1|k−1 at time t k−1 and the measurement z k .
Output: posterior mean ˆξ k|k and covariance matrix P k|k
at time t k .
set ˆξ 0 k|k−1 = ˆξ k−1|k−1 and P 0 k|k−1 = P k−1|k−1.
compute the Jacobian B =
[
Dξ ′b(ξ, u k−1 , w) D w ′b(ξ, u k−1 , w) ]∣ ∣
ξ=ˆξk−1|k−1 ,w=0 .
compute the predicted statistics
ˆξ k|k−1 = b(ˆξ 0 k|k−1, u k−1 , 0),
P k|k−1 = B diag(P 0 k|k−1 , Q(∆ k))B ′ .
compute the innovation covariance
S k = HP k|k−1 H ′ + R.
compute the gain matrix K k = P k|k−1 H ′ S −1
k .
compute the posterior statistics
ˆξ k|k = ˆξ k|k−1 + K k (z k − Hˆξ k|k−1 ),
P k|k = P k|k−1 − K k S k K ′ k.
Ω k = b(n) ˆP k−1 , (48)
where b(n) is a scaling factor and ˆP k−1 is the weighted sample
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. NUMERICAL SIMULATIONS AND RESULTS
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 z o 1 of the ownship
is 10.0 km. 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

Algorithm 2: A recursion of the UKF using MSC.
Input: posterior mean ˆξ k−1|k−1 and covariance matrix
P k−1|k−1 at time k − 1 and the measurement z k
Output: posterior mean ˆξ k|k and covariance matrix P k|k
at time k
construct the matrices
Σ = [ 0 6,1
√
(12 + κ)Pk−1|k−1 0 6,6
− √ (12 + κ)P k−1|k−1 0 6,6
]
Ω = [ 0 6,1 0 6,6
√
(12 + κ)Q(∆k )
0 6,6 − √ (12 + κ)Q(∆ k ) ]
for b = 0, . . . , 24 do
compute the sigma points Ξ k,b = ˆξ k−1|k−1 + σ b ,
W k,b = ω b where σ b and ω b are the (b + 1)th
columns of Σ i and Ω, respectively.
compute the transformed sigma point and covariance
matrix:
E k,b = b(Ξ k,b , u k−1 , W k,b )
end
compute the predicted statistics
∑24
ˆξ k|k−1 = w b E k,b
b=0
24
∑
P k|k−1 = w b (E k,b − ˆξ k|k−1 )(E k,b − ˆξ k|k−1 ) ′
b=0
compute the innovation covariance
S k = HP k|k−1 H ′ + R.
compute the gain matrix K k = P k|k−1 H ′ S −1
k .
compute the corrected statistics
ˆξ k|k = ˆξ k|k−1 + K k (z k − Hˆξ k|k−1 )
P k|k = P k|k−1 − K k S k K ′ k.
ż t 1. 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 (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, 0.0001) m 2 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
Algorithm 3: A recursion of the BF using MSC.
Input: a weighted sample {wk−1 i , ξi k−1} from the
posterior at time t k−1 and the measurement z k .
Output: a weighted sample {wk i , ξi k} from the posterior
at time t k .
select indices c(1), . . . , c(n) according to the weights
wk−1 1 , . . . , wn k−1 .
for i = 1, . . . , n do
draw wk i ∼ N(0, Q(∆ k))
set ξ i k = b(ξ c(i)
k−1 , u k, wk i )
compute the un-normalized weight
˜w
k i = N(z k; Hξ i k, R)
end
compute the normalized weights
/
∑ n
wk i = ˜w k
i ˜w j k .
j=1
compute the state estimate
n∑
ˆx k = wkf i MSC(ξ C i k).
i=1
TABLE I
INITIAL PARAMETERS OF TARGET.
Variable Value
ρ 1 (km) 138.0
β 1 (deg) 45.0
ɛ 1 (deg) -0.415
z1 t (km) 9.0
s 1 (m/s) 297.0
c 1 (deg) -135.0
ż1 t (m/s) 0.0
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

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] 108 0 CV 0
TABLE IV
FINAL RMS POSITION ERROR IN METERS.
Algorithm sdv (mrad) sdv (mrad) sdv (mrad)
1.0 15.0 35.0
CEKF 0.472 3.363 4.780
CUKF 0.468 3.308 4.658
CBF 0.527 4.173 5.384
EKF-MSC 0.462 2.413 3.333
UKF-MSC 0.462 2.533 3.499
BF-MSC 1.355 3.575 5.286
Y (m)
10 x 104 Target and Ownship Trajectories
9
8
7
6
5
4
3
2
1
0
Target
Ownship
0 2 4 6 8 10
Fig. 2.
X (m)
Target and ownship trajectories.
x 10 4
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
TIME-AVERAGED RMS POSITION ERROR IN METERS.
Algorithm sdv (mrad) sdv (mrad) sdv (mrad)
1.0 15.0 35.0
CEKF 0.650 5.786 12.00
CUKF 0.644 5.743 12.02
CBF 0.694 6.183 12.09
EKF-MSC 0.629 4.773 10.65
UKF-MSC 0.629 4.827 10.48
BF-MSC 1.310 5.689 11.71
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.
TABLE V
CPU TIMES OF FILTERING ALGORITHMS FOR THE MEASUREMENT
STANDARD DEVIATION OF 15 MRAD.
Algorithm CPU (s) Relative CPU
CEKF 0.036 1.00
CUKF 0.110 2.97
CBF 6.280 175.22
EKF-MSC 0.980 27.39
UKF-MSC 0.820 22.86
BF-MSC 15.600 434.90
IX. CONCLUSIONS
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ér Rao lower bound (PCRLB) for the filtering problem
which represents the best achievable accuracy [13]. Then the

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.
REFERENCES
[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ébec, 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éhard and J-P. Le Cadre, Closed-form Posterior Cramér-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éhard and J-P. Le Cadre, Closed-form posterior Cramér-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ürzeler and H. R. Künsch, 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. 247-
271, 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.