12th International Conference on Information Fusion Seattle, WA, USA, July 6-9, 2009
Rao-Blackwellised variable rate particle filters Mark R. Morelande Melbourne Systems Laboratory The University of Melbourne Parkville, Australia
Neil Gordon ISR Division Defence Science and Technology Organisation Edinburgh, Australia
[email protected]
[email protected]
of motion to be modelled using a single model. This is akin to a piecewise linear fit to a curved line. Each segment of the fit has the same parametric model but the length of each segment should depend on how quickly the gradient of the curve is changing: long segments can be used when the curve is straight and short segments are required when curve bends. This is illustrated in Figure 1 which shows variable and fixed rate piecewise linear fits to a sinusoid. Since the curve, or trajectory, is unknown in target tracking suitable segment lengths cannot be selected a priori. Rather the segment lengths must be selected adaptively as suggested by the measurements. In other words, the parameters of the segments along with their durations must be estimated.
Abstract – Variable rate particle filters have recently emerged as an alternative to multiple model techniques for tracking highly manoeuvrable targets. The basic idea of variable rate methods is to apply local fits to segments of the target trajectory of variable length. Both the fit parameters and the length of the segments need to be estimated. Approximately optimal Bayesian estimation of these quantities can be performed using particle filters. In this paper a Rao-Blackwellised variable particle filter is developed which offers significant performance improvements over existing methods for a certain class of models. This is demonstrated via Monte Carlo simulations for a benchmark tracking problem. Keywords: Manoeuvring target tracking; Variable rate filters; Particle filtering.
1
Introduction
∗
The usual approach to manoeuvring target tracking is to assume that the target motion at any given time can be adequately described by one of a finite set of motion models. This multiple model approach to manoeuvring target tracking requires estimation of a discrete manoeuvring mode in addition to the usual kinematic parameters such as position and velocity. Optimal estimation, in the Bayesian sense, is impossible even if all motion models are linear and Gaussian. Of the number of computationally efficient approximations which have been proposed [1], the interacting multiple model filter (IMMF) [2] achieves the best trade-off between accuracy and computational expense. In multiple model approaches the evolution of the target kinematic parameters is tied to the measurement sampling rate, i.e., motion models are assumed to remain in effect from one measurement to the next. Variable rate techniques remove this restriction so that the parameters determining the evolution of the kinematic state are fixed for a period which is not necessarily the same as the measurement sampling period [3]. In principle, variable rate state updates allow different types
978-0-9824438-0-4 ©2009 ISIF
∗∗∗
∗ ∗ ∗
∗
∗
∗ ∗
(a) Constant rate
∗
∗ ∗
(b) Variable rate
Figure 1: Piecewise linear fits (dashed) to a sinusoid (solid) using (a) constant rate and (b) variable rate approaches. Bayesian inference in variable rate models cannot be performed exactly but can be performed approximately using sequential Monte Carlo methods, or particle filters (PFs) [4]. This has been demonstrated in [3]. In this paper an improvement to the existing variable rate PFs, applicable for a certain class of models, is derived based on Rao-Blackwellisation [5]. The performances of existing and Rao-Blackwellised variable rate PFs and the IMMF are analysed for a benchmark tracking problem using Monte Carlo simulations. The paper is organised as follows. The variable rate model is described in Section 2. Variable rate PFs are reviewed in Section 3 and a Rao-Blackwellised variable
1
rate PF is derived Section 4. The results of a simulation analysis are presented in Section 5.
2
the kinematic parameter θk . This extends the notation used in (1) for evolution of the state over a single segment. Note that τk+1 and v k+1 are not included in θk as they are not required to calculate the kinematic state in any of the k segments although τk+1 is required to specify the length of the kth segment. It is better to leave the final state update time unspecified in order to obtain diversity in the segment lengths. Care must be taken to ensure that, when this state update time is sampled, it does not modify previous likelihoods. The appropriate conditions can easily be incorporated into the sampling process. This will be discussed in more detail below.
Modelling
Let x(t) denote a vector of kinematic variables, such as position and velocity, at time t ∈ R+ . The kinematic state vector is assumed to evolve in a piecewise fashion such that, for t ∈ (τk , τk+1 ], k = 1, 2, . . ., x(t) = a(x(τk ), v k , t, τk )
(1)
where the inputs v k ∼ fV , k = 1, 2, . . . are independent random variables. In a constant rate model, the times τ1 , τ2 , . . . are fixed and generally tied to the measurement sampling rate. In a variable rate model the state update times are random parameters which are decoupled from the measurement sampling rate and must be estimated. The prior model for the state update times is τk+1 |τk ∼ fτ (·|τk ), k = 1, 2, . . . (2)
3.1
The PF approximation to the posterior PDF of the kinematic state trajectory at time tm−1 is comprised of trajectory parameter samples θ1k1 , . . . , θnkm−1 , where n m−1
i n is the sample size and km−1 ≥ 1 is the number of segments in the ith sample up to time tm−1 , and corre1 n , . . . , wm−1 . Note that τki i < sponding weights wm−1 m−1 tm−1 . Using the PF approximation to the PDF at time tm−1 and Bayes rule, an approximation to the posterior PDF at time tm can be obtained as
Initially, x(0) ∼ π0 and τ1 = 0. Noisy partial measurements y 1 , . . . , y M of the kinematic state are made at discrete time instants t1 < t2 < · · · < tM . Let xm = x(tm ) and x0:m denote the trajectory of states up to time tm . The conditional measurement PDF can then be written as, p(y 1:M |x0:M ) =
M Y
ℓ(y m |xm )
General framework for variable rate particle filtering
p(km , θkm |y 1:m ) ≈ C ℓ(ym |akm (tm , θkm )) n X i i , θ iki wm−1 f km , θkm |km−1 ×
(3)
m−1
i=1
m=1
(4)
where C is a normalizing constant. The PDF f in (4) is defined by the transition PDF fτ for the state upi date times, with the condition τkm−1 +1 > tm−1 , and the PDF fV of the process noise input. Samples from fτ satisfying the required condition can be obtained i by rejection sampling. The condition on τkm−1 +1 en-
The variable rate model adopted here is the same as that used in the performance analysis of [3]. The goal of variable rate filtering is to recursively estimate the kinematic state xm given the measurements y 1:m . Estimation of the kinematic state xm actually involves estimation of the quantities which describe the state trajectory, i.e., the initial kinematic state x0 , the sequence of update times τ1:K and the sequence of process noise inputs v 1:K . Optimal Bayesian estimation requires the posterior PDF which cannot be calculated for any but the most trivial variable rate models. Particle filtering approximations are described in the following sections.
i i i sures that akm−1 (tj , θiki ) for i +d (tj , θ km−1 +d ) = akm−1 m−1 d = 1, 2, . . ., j = 1, . . . , m − 1 so that the likelihood of the state trajectory is unchanged for the first m − 1 measurements. The mixture (4) can be re-written as, i , for i = 1, . . . , n, km ≥ km−1 i ℓ(y m |akm (tm , θkm )) p(km , θkm , i|y 1:m ) ∝ wm−1 i , θiki (5) × f km , θkm |km−1 m−1
3
Variable rate particle filtering
where i is an index on the mixture, also referred to as an auxiliary variable [6]. A Monte Carlo approximation to (5) is produced by drawing samples of the auxiliary variable, segment number and trajectory parameters from an importance density of the form
Particle filters (PFs) are a class of techniques which compute a sequential Monte Carlo approximation to the posterior PDF [4]. In variable rate filtering the object of interest is the state trajectory which is completely described by an initial kinematic state, a sequence of state update times, and a sequence of process noise inputs. For a kinematic state trajectory composed of k ′ segments let θ k = [τ1:k , v ′1:k , x(0)]′ denote the variables which specify x(t) for t ≥ τ1 . Let x(t) = ak (t, θk ) denote the value of the kinematic state at time t for
i q(km , θkm |i) q(km , θkm , i) = ψm
Pn
(6)
i where i=1 ψm = 1. The approximation to the posterior PDF of the segment number and trajectory parameters is obtained by simply discarding the auxiliary varii ables. The weights of the samples (km , θikm i , j(i)) ∼ q
2
Algorithm 1 Variable rate bootstrap filter 1: for i = 1, . . . , n do 2: draw xi0 ∼ π0 , v i1 ∼ fV and set w0i = 1/n, k0i = 1 and τ1i = t0 = 0. 3: for m = 1, . . . , M do 4: set j(0) = 1 and draw u ∼ U[0,1] . Pi a 5: for i = 1, . . . , n do cim−1 = a=1 wm−1 end for 6: for i = 1, . . . , n do 7: set j(i) = j(i − 1). j(i) 8: while cm−1 < (u + i − 1)/n do j(i) ← j(i) + 1 end while 9: draw samples i i i km , τki m i , v k i , xm m j(i) j(i) j(i) j(i) = sample prior km−1 , τ j(i) , v j(i) , xm−1 , tm−1 , tm
are j(i)
i wm ∝ wm−1
×
i i j(i) j(i) f km , θ km i |km−1 , θ j(i) km−1 j(i) i i ψm q km , θkm i |j(i) (7)
i tm , θikm ℓ y m |akm i
The generic scheme described here can be implemented in many ways depending on how the importance density (6) is selected. Two options are discussed below.
3.2
Bootstrap filter
The variable rate bootstrap filter (VRBF) involves using the importance density (6) with i i ψm = wm−1
km−1
(8)
i , θiki q(km , θkm |i) = f km , θkm |km−1
km−1
i calculate the un-normalised weight w ˜m = i ℓ(y m |xm ). for i = 1, . . . , n do calculate the normalised weight , n X j i i w ˜m wm =w ˜m
(9)
10:
According to (8), sample indices are selected according to the weights calculated from the first m − 1 measurements. This procedure, known as resampling [7], concentrates the sample set in areas of interest by removing unlikely samples. The weight update is obtained, using (7), as i i wm ∝ ℓ(y m |akm i (tm , θ i )). (10) km
11:
The VR-BF is simple to implement and generally applicable but, because it does not use the current measurement for sampling of auxiliary variables or states, it tends to perform poorly in certain situations, most notably when the measurements are accurate. The operation of the VR-BF is summarised by Algorithm 1. The function sample prior is given in Algorithm 2. The VR-BF is similar to the algorithm used in [3]. The main difference occurs in the selection of sample indices. A biased scheme was used for this purpose in [3].
Algorithm 2 Sampling from the prior Input: Current trajectory length k, start time of kth segment τ , process noise input for kth segment v, kinematic state at last measurement time x, last measurement time t, current measurement time t∗ . Output: Updated trajectory length k ∗ , start time of k ∗ th segment τ ∗ , process noise input for k ∗ th segment v ∗ , kinematic state at current measurement time x∗ . 1: set υ = t, x∗ = x, τ ∗ = τ , v ∗ = v and k ∗ = k. 2: draw ρ ∼ fτ (·|τ, ρ > t) 3: while ρ < t∗ do 4: calculate x∗ ← a(x∗ , v ∗ , ρ, υ). 5: set υ ← τ ∗ ← ρ. 6: increment the trajectory length k ∗ ← k ∗ + 1. 7: draw v ∗ ∼ fV . 8: draw ρ ∼ fτ (·|τ ∗ ). 9: calculate x∗ ← a(x∗ , v ∗ , t∗ , υ).
m−1
3.3
12:
j=1
Auxiliary bootstrap filter
A slight modification of the VR-BF allows the current measurement to influence sampling of the auxiliary variables. The variable rate auxiliary bootstrap filter (VR-ABF) uses an importance density of the form (6) with i i ψm ∝ wm−1 ℓ y m |aκim tm , µiκim (11) i i , θ ki (12) q(km , θkm |i) = f km , θkm |km−1 m−1
only to calculate the first-stage weights. The likelihoods of these samples act as indicators of the fitness of the samples as determined by the current measurement. Using these likelihoods as update factors applied to the previous weights for the first-stage weights enables the current measurement to influence selection of the sample indices. The second set of trajectory parameter samples are then drawn from the transition PDF conditional on the selected sample values. An
i where (κim , µiκi ) ∼ f (·|km−1 , θiki ). The weights of m−1 m the samples drawn using this procedure can be found, using (7), as i i tm , θikm /ℓ y m |aκim tm , µiκim wm ∝ ℓ y m |akm i (13) The VR-ABF involves drawing two sets of trajectory parameter samples. The first set of samples are drawn
3
Algorithm 3 Variable rate auxiliary bootstrap filter 1: for i = 1, . . . , n do 2: draw xi0 ∼ π0 , v i1 ∼ fV and set w0i = 1/n, k0i = 1 and τ1i = t0 = 0. 3: for m = 1, . . . , M do 4: for i = 1, . . . , n do 5: draw samples ˜ im κ, ϕ, ν, x i = sample prior km−1 , τki i , v iki , xim−1 , tm−1 , tm
important property of the VR-ABF is that it can be implemented with the same generality as the VR-BF, requiring only the ability to sample from the transition PDF for the trajectory parameters and compute the likelihood. An interesting interpretation of the VR-ABF is provided via comparison with the VR-BF. Although a recursion of the VR-BF, as given by Algorithm 1, involves selecting sample indices and then drawing samples, it can also be implemented by drawing samples then selecting sample indices, i.e., resampling. When implemented in this manner the VR-BF recursion and the first two steps of the VR-ABF recursion are identical. However, rather then proceeding with the set of particles selected in the resampling step, which will inevitably contain duplicate particle values, the VR-ABF obtains a diverse sample set by drawing again from the transition PDF conditional on the previous values of the selected particles. These re-sampled samples are then weighted by a likelihood ratio. From this point-of-view the VR-ABF is a VR-BF with a move step and an appropriate adjustment of the sample weights. MCMC techniques have been proposed for the same purpose [8], although in the case of MCMC moves the proposed move is not always accepted. The VR-ABF is summarised by Algorithm 3.
4
m−1
6: 7: 8:
m−1
compute the likelihood ϑim = ℓ(y m |˜ xim ). for i = 1, . . . , n do i compute , the normalised first-stage ψm = n X j i wm−1 ϑjm wm−1 ϑim j=1
compute the cumulative weight cim = Pi j j=1 ψm . 10: set j(0) = 1 and draw u ∼ U[0,1] . 11: for i = 1, . . . , n do 12: Set j(i) = j(i − 1). j(i) 13: while cm < (u + i − 1)/n do j(i) ← j(i) + 1 end while 14: draw samples i i i km , τki m i , v k i , xm m j(i) j(i) j(i) j(i) = sample prior km−1 , τ j(i) , v j(i) , xm−1 , tm−1 , tm 9:
Rao-Blackwellised variable rate particle filtering
km−1
In many practical problems the vector of parameters can be partitioned in such a way that the posterior PDF of certain parameters conditional on the remaining parameters can be computed in closed form. This property can be exploited to reduce the difficulty of numerically approximating the posterior PDF. The formal proof of this is provided by the Rao-Blackwell (RB) theorem [5], hence the use of the term Rao-Blackwellisation to describe the procedure. In variable rate filtering the parameter of interest is θk , the vector of parameters which define a trajectory composed of k segments. Let θ k = [φ′k , ϑ′k ]′ and expand the posterior PDF as
15:
16: 17:
km−1
i calculate the un-normalised weight w ˜m = j(i) i ℓ(y m |xm )/ϑm . for i = 1, . . . , n do i calculate , n the normalised weight wm = X j i w ˜m w ˜m j=1
Thus the RB-VRPF approximation to the posterior PDF involves drawing samples of [k, φ′k ]′ , which can be done as described in Section 3, and computing the posterior PDF of ϑk conditional on each sampled value of [k, φ′k ]′ . In this section a general model under which the latter computation can be performed in closed-form is described and a RB-VRPF for this model is developed.
p(k, θk |y 1:m ) = p(ϑk |k, φk , y 1:m )p(k, φk |y 1:m ) (14) If the posterior PDF of the partition ϑk conditional on k and φk is available in closed-form then it is only necessary to approximate the posterior PDF of [k, φ′k ]′ to obtain an approximation to the posterior PDF of [k, θ′k ]′ . In the context of particle filtering this means that only samples of the trajectory length k and the pa1 n rameters φk are required. Let km , . . . , km denote tra1 n jectory length samples and φkm denote the 1 , . . . , φkn m parameter samples. The RB-VRPF approximation to the posterior PDF is X i wm p(ϑk |k, φik , y 1:m )δ(φk −φik ) p(k, θk |y 1:m ) ≈
4.1
A partially linear/Gaussian model
Let x(t) = [ξ(t)′ , ζ(t)′ ]′ denote a partitioning of the kinematic state and v k = [s′k , r ′k ]′ denote a partitioning of the input vector such that the state evolution
i =k} {i:km
4
equation (1) can be re-written as, for t > τk ,
The posterior mean and covariance matrix of ξ(tm ) conditional on φkm can be found as
ξ(t) = B(ζ(τk ), r k , t, τk )ξ(τk ) + G(ζ(τk ), rk , t, τk )sk (15) ζ(t) = e(ζ(τk ), r k , t, τk )
ξˆm|m (φkm ) = ξˆm|m−1 (φkm ) + Ψm (φkm )ǫm (φkm ) (22)
(16)
C m|m (φkm ) = C m|m−1 (φkm ) − Ψm (φkm )HC m|m−1 (φkm )
The prior PDF for the kinematic state is assumed to be of the form π0 (ξ(0), ζ(0)) = fZ0 (ζ(0))N (ξ(0); ξˆ0 (ζ(0)), C 0 (ζ(0))) (17) Let φk = [ζ(0), τ1:k , r ′1:k ] and ϑk = [ξ(0), s1:k ]. It is assumed that fV (r k , sk ) = N (sk ; µS (r k ), ΣS (r k ))fR (r k )
where ǫm (φkm ) = y m − H ξˆm|m−1 (φkm ) ′
Ψm (φkm ) = C m|m−1 (φkm )H S m (φkm )−1
(18)
(19)
The key to analytic computation of the posterior PDF of ϑk given [k, φ′k ]′ is that the evolution and measurement of ξ(t) conditional on [k, φ′k ]′ is described by a linear-Gaussian dynamic system. In most cases it is not actually required to compute the posterior PDF of the complete sequence ϑk of trajectory parameters. Instead the filtering density of the kinematic state will usually be of interest. With this in mind, a recursion for the posterior PDF of the linear part ξ(t) of the kinematic state is developed below. This greatly reduces the storage requirements of the algorithm.
4.2
5
(25)
Performance analysis
The performances of the various VR-PF implementations described in Sections 3 and 4 are analysed in this section using Monte Carlo simulations. The analysis includes a comparison with the interacting multiple model filter (IMMF). The trajectory used in the performance analysis is one of the benchmark trajectories introduced in [9] and shown in Figure 2. The asterisk denotes the position of the target at the starting time t = 0. The target reaches the end of the trajectory at time t = 188s. In this scenario the manoeuvres include approximately constant rate turns with both constant speed and rapidly changing speed. These characteristics can be seen in the plots of turn rate and speed against time shown in Figure 3. Noisy measurements of the target position in Cartesian coordinates are made at intervals of 0.5s with covariance matrix R = 225I 2 . The kinematic state in the VR-PFs is composed of position in Cartesian coordinates and velocity in polar coordinates. The kinematic state at time t is x(t) = [x(t), y(t), ϕ(t), u(t)]′ where (x(t), y(t)) is the position in Cartesian coordinates, ϕ(t) is the heading and u(t) is the speed. The inputs to the state evolution equation (1) are a turn rate ωk and a linear acceleration αk . This model satisfies the requirements for use of the RB-VRBF. In the notation of Section 4, ξ(t) = [x(t), y(t), u(t)]′ is the linear part of the kinematic state, ζ(t) = ϕ(t) is the nonlinear part of the kinematic state, rk = ωk and sk = αk . The matrices
Derivation of the Rao-Blackwellised filter
The posterior PDF of ξ(t) given [k, φ′k ]′ is Gaussian and can be computed using a Kalman filter recursion with a slightly modified prediction step to account for the variability in the number of state updates. Let ξˆm−1|m−1 (φkm−1 ) and C m−1|m−1 (φkm−1 ) denote the posterior mean and covariance matrix, respectively, of ξ m−1 = ξ(tm−1 ) conditional on φkm−1 . Let υ0 = tm−1 , υj = τkm−1 +j , j = 1, . . . , km − km−1 and υkm −km−1 +1 = ˆ j |m−1) tm . The prior mean at time υj is denoted as ξ(υ ˆ ˆ so that ξ m−1|m−1 (φkm−1 ) = ξ(υ0 |m − 1). A similar notation is used for the prior covariance matrix. Then, for j = 0, . . . , km − km−1 , ˆ j |m − 1) + Gj µ (rk ˆ j+1 |m − 1) = B j ξ(υ ξ(υ m−1 +j ) S (20) C(υj+1 |m − 1) = B j C(υj |m − 1)B ′j + Gj ΣS (r km−1 +j )G′j
(24)
with S m (φkm ) = HC m|m−1 (φkm )H ′ + R. Eqs. (20), (21), (22) and (23) define a recursion for the posterior mean and covariance matrix of ξ m conditional on [km , φ′km ]′ . Either of the VR-PFs of Section 3 can be used to sequentially produce samples of [km , φ′km ]′ . The simpler of the two, the VRBF, is used here. The resulting filter, referred to as the Rao-Blackwellised VRBF (RB-VRBF) is summarised by Algorithm 4. The procedure for sampling from the prior and updating the kinematic state statistics is given by Algorithm 5.
and that the likelihood satisfies ℓ(y m |xm ) = N (y m ; Hξ m , R)
(23)
(21)
where B j = B(ζ(υj ), r km−1 +j , υj+1 , υj ) and similarly for Gj . The prior mean and covariance matrix, denoted ξˆm|m−1 (φkm ) and C m|m−1 (φkm ), respectively, are found by setting j = km − km−1 in (20) and (21).
5
Algorithm 4 Rao Blackwellised variable rate bootstrap filter 1: for i = 1, . . . , n do i 2: draw ζ i0 ∼ fZ0 , ri1 ∼ fR and set ξˆ0|0 = ξˆ0 (ζ i0 ), C i0|0 = C 0 (ζ i0 ), w0i = 1/n, k0i = 1 and τ1i = t0 = 0. 3: for m = 1, . . . , M do 4: set j(0) = 1 and draw u ∼ U[0,1] . Pi a 5: for i = 1, . . . , n do cim = a=1 wm−1 end for 6: for i = 1, . . . , n do 7: set j(i) = j(i − 1). j(i) 8: while cm−1 < (u + i − 1)/n do j(i) ← j(i) + 1 end while 9: draw samples i i i ˆi km , ζ im , τki m i , r ki , ξ m|m−1 , C m|m−1 m j(i) j(i) j(i) = sample prior RB km−1 , ζ im−1 , τ j(i) , r j(i) , km−1 km−1 j(i) j(i) ˆ m−1|m−1 , C m−1|m−1 , tm−1 , tm x 10:
11:
Algorithm 5 Sampling from the prior for Rao Blackwellised filtering Input: Current trajectory length k, nonlinear kinematic state ζ, start time of kth segment τ , trajectory parameter for kth segment r, linear kinematic state posterior mean ξˆ and covariance matrix C at last measurement time, last measurement time t, current measurement time t∗ . Output: Updated trajectory length k ∗ , updated nonlinear kinematic state ζ ∗ , start time of k ∗ th segment τ ∗ , trajectory parameter for k ∗ th segment r∗ , lin∗ ear kinematic state prior mean ξˆ and covariance matrix C ∗ at current measurement time t∗ . ∗ ˆ ˆ C ∗ = C, ζ ∗ = ζ, τ ∗ = τ , r∗ = r 1: set υ = t, ξ = ξ, ∗ and k = k. 2: draw ρ ∼ fτ (·|τ, ρ > t) 3: while ρ < t∗ do 4: calculate B ∗ = B(ζ ∗ , r∗ , ρ, υ) and G∗ = G(ζ ∗ , r ∗ , ρ, υ). ∗ ∗ 5: calculate ξˆ ← B ∗ ξˆ + G∗ µS (r ∗ ). 6: calculate C ∗ ← B ∗ C ∗ B ∗′ + G∗ ΣS (r ∗ )G∗′ . 7: calculate ζ ∗ ← e(ζ ∗ , r ∗ , ρ, υ). 8: set υ ← τ ∗ ← ρ. 9: increment the trajectory length k ∗ ← k ∗ + 1. 10: draw r ∗ ∼ fR . 11: draw ρ ∼ fτ (·|τ ∗ ). ∗ 12: calculate B = B(ζ ∗ , r∗ , t∗ , υ) and G∗ = ∗ ∗ ∗ G(ζ , r , t , υ). ˆ∗ ← B ∗ ξˆ∗ + G∗ µ . 13: calculate ξ S ∗ ∗ ∗ ∗′ ∗ ∗′ 14: calculate C ← B C B + G ΣS (r ∗ )G . ∗ ∗ 15: calculate ζ ← e(ζ , r ∗ , t∗ , υ).
calculate Ψim = C im|m−1 H ′ (S im )−1 , S im = i ˆ im = H ξˆm|m−1 . HC im|m−1 H ′ + R and y calculate the posterior statistics i i ˆ im ) ξˆm|m = ξˆm|m−1 + Ψim (y − y
C im|m = C im|m−1 − Ψim HC im|m−1 12: 13: 14:
i calculate the un-normalised weight w ˜m = i i ˆ N (y m ; y m , S m ). for i = 1, . . . , n do calculate the normalised weight , n X j i i w ˜m wm = w ˜m
35 30 25 y−position (km)
j=1
20 15 10
5 describing the evolution of ξ(t) in (15) are, for ω 6= 0, 0 1 0 U (ϕ, ω, τ ) −5 B(ϕ, ω, t + τ, t) = 0 1 −W (ϕ, ω, τ ) , −10 0 0 1 35 40 45 50 55 60 65 70 x−position (km) [τ sin(ϕ + ωτ ) + U (ϕ, ω, τ )]/ω G(ϕ, ω, t + τ, t) = [−τ cos(ϕ + ωτ ) + W (ϕ, ω, τ )]/ω , Figure 2: Target trajectory used for the performance τ analysis. The asterisk denotes the starting position. where
U (ϕ, ω, τ ) = [cos(ϕ + ωτ ) − cos ϕ]/ω, W (ϕ, ω, τ ) = [sin(ϕ + ωτ ) − sin ϕ]/ω.
target manoeuvres. Let ρ = 180ω/π denote the turn rate in ◦ /s. Then, the turn rate PDF is
For ω = 0, the elements of B and G are found using L’Hˆopital’s rule. The distributions used for the turn rate and acceleration should account for the expected
fP (ρ) = [2N (ρ; 0, 0.01)+N (ρ; 10, 16)+N (ρ; −10, 16)]/4 The use of a mixture distribution for the turn rate al-
6
2
10
0.1 0 −0.1 −0.2 0
20
40
60
80
100
120
140
160
180
20
40
60
80 100 Time (s)
120
140
160
180
450 Speed (m/s)
VR−BF VR−ABF RB−VRBF IMMF
0.2
Time−averaged RMS position error
Turn rate (rad/s)
0.3
400 350 300 250
1
200 0
10 2 10
Figure 3: (Top) Turn rate and (Bottom) speed of the target plotted against time.
3
4
10 Sample size
10
Figure 4: Time-averaged RMS position error plotted against sample size for the VR-BF, VR-ABF and RBVRBF. The time-averaged RMS position error of the IMMF is also shown.
lows for greater flexibility in modelling different types of target motion. The PDF for the acceleration input is fA (α) = N (α; 0, 144). The state update times satisfy (τk − τk−1 − 1) ∼ G(1.5, 1/4) where G(α, β) is the Gamma distribution with shape parameter α and rate parameter β. Note that a lower bound of 1s is imposed on the time between successive state updates. The IMMF is implemented with a uniform motion model and several turn models. The target state vector at time tm is xm = [xm , ym , ϕm , um , ωm , u˙ m ]′ . Under the jth manoeuvring mode, xm |xm−1 ∼ N (f j (xm−1 ), Qj (xm−1 )). It is desired to model three different types of motion: uniform motion, turning motion with a constant speed and turning motion with a changing speed. Transitional modes are added to facilitate transitions between these types of motion. This results in five motion models. The IMMF and the VR-BF, VR-ABF and RB-VRBF with sample sizes between 100 and 10 000 are applied to the benchmark tracking problem. The RMS position error is computed over 250 realisations for each algorithm and sample size. Figure 4 shows the RMS position error, averaged over time, plotted against sample size for the three variable rate PFs. The time-averaged RMS position error of the IMMF is also shown. Of the VR-PFs, the best performance is achieved by the RB-VRBF. It outperforms the IMMF with sample sizes greater than 1000 while the VR-BF and VR-ABF have RMS position errors greater than the IMMF even with a sample size of 10 000. It is of interest to examine how the various algorithms react to target manoeuvres. This can be done by looking at estimation errors as a function of time. Figure 5 shows the RMS position errors of the various algorithms plotted for times between 100 s and 150 s. Figure 3 shows that the target motion is quite complicated during this period. The speed decreases at an approximately constant rate until about t = 125 after which
time it increases at an approximately constant rate. A turn is executed between t = 115 and t = 125. The situation is made difficult by the fact that there is a transition from turning to straight motion at the same time there is a sudden change in the target acceleration. It can be seen that the RM-VRBF has a lower error than the IMMF during periods of fixed motion type and responds much better than the IMMF to motion transition that occurs around t = 125. Interestingly, there is little difference between the RB-VRBF and the IMMF around t = 115 at the onset of the turn. In fact the peak error of the IMMF due to the turn is lower than for the RB-VRBF although the IMMF takes longer to recover.
30
VR−BF VR−ABF RB−VRBF IMMF
RMS position error
25 20 15 10 5 0 100
110
120
130
140
150
Time
Figure 5: RMS position error plotted against time for the IMMF and the VR-BF, VR-ABF and RB-VRBF with a sample size of 10 000.
7
6
Conclusions
[2] H. Blom and Y. Bar-Shalom, “The interacting multiple model algorithm for systems with Markovian switching coefficients,” IEEE Transactions on Automatic Control, vol. 33, no. 8, pp. 780–783, 1988.
Variable rate approaches have been considered for tracking of manoeuvring targets. A Rao-Blackwellised variable rate particle filter was proposed which improves significantly upon existing variable rate particle filters and slightly upon the interacting multiple model filter (IMMF) for a benchmark tracking problem. Previous publications have reported large improvements for variable rate particle filters compared to a particle filter implementation of the IMMF [3]. Improvements of the same magnitude were not observed here. One reason for this discrepancy may be the different models used in the IMMF implementations. Godsill et al used motion models with fixed turn rates. This can result in poor performance when the target turns at a rate not matched by the model, particularly in a particle filter implementation of the IMMF. The models used here for the IMMF do not specify a particular turn rate but instead include the turn rate as a parameter in the state vector which is estimated in turning motion models. This provides the flexibility to track target manoeuvres at different turn rates and may explain the fact that only marginal improvements are achieved for the variable rate approaches compared to the IMMF. It is anticipated that the performance improvement offered by variable rate approaches compared to the IMMF will increases as the measurement sampling period increases. This is because larger sampling periods have the effect of making target manoeuvres seem more sudden, thus increasing the “effective” manoeuvrability of the target.
[3] S. Godsill, J. Vermaak, W. Ng, and J. Li, “Models and algorithms for tracking of manoeuvring objects using variable rate particle filters,” Proceedings of the IEEE, vol. 95, no. 5, pp. 925–952, 2007. [4] A. Doucet, N. de Freitas, and N. Gordon, Eds., Sequential Monte Carlo Methods in Practice. New York: Springer-Verlag, 2001. [5] G. Casella and C. Robert, “Rao-Blackwellisation of sampling schemes,” Biometrika, vol. 83, no. 1, pp. 81–94, 1996. [6] M. Pitt and N. Shephard, “Filtering via simulation: auxiliary particle filters,” Journal of the American Statistical Association, vol. 94, pp. 590–599, 1999. [7] N. Gordon, D. Salmond, and A. Smith, “Novel approach to nonlinear/non-Gaussian Bayesian state estimation,” IEE Proceedings Part F, vol. 140, no. 2, pp. 107–113, 1993. [8] W. Gilks and C. Berzuini, “Following a moving target- Monte Carlo inference for dynamic Bayesian models,” Journal of the Royal Statistical Society B, vol. 63, no. 1, pp. 127–146, 2001. [9] W. Blair, G. Watson, T. Kirubarajan, and Y. BarShalom, “Benchmark for radar allocation and tracking in ECM,” IEEE Transactions on Aerospace and Electronic Systems, vol. 34, no. 4, pp. 1097–1114, 1998.
References [1] Y. Bar-Shalom and X.-R. Li, Estimation and Tracking: Principles, Techniques and Software. Artech House, 1993.
8
