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

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