A Spline Filter for Multidimensional Nonlinear State Estimation
14th International Conference on Information Fusion Chicago, Illinois, USA, July 5-8, 2011
A Spline Filter for Multidimensional Nonlinear State Estimation Xiaofan He
Ratnasingham Tharmarasa
Donna Lynn Kocherry
Dept. of ECE McMaster University Hamilton, Canada. [email protected]
Dept. of ECE McMaster University Hamilton, Canada. [email protected]
Dept. of ECE McMaster University Hamilton, Canada. [email protected]
Bhashyam Balaji
Thiagalingam Kirubarajan
Radar Systems Section Defence Research and Development Canada Ottawa, Canada. [email protected]
Dept. of ECE McMaster University Hamilton, Canada. [email protected]
Abstract—The problem of nonlinear/non-Gaussian filtering has generated significant interest in the literature. Sequential Monte Carlo (SMC)/Markov Chain Monte Carlo (MCMC) approaches are the most commonly used. The success of nonlinear/nonGaussian filtering depends on the accurate representation of the pdf of the system state as well as the likelihood function. However, the commonly used Monte Carlo approaches only provide weighted samples at discrete points in the state space. In this paper, a comprehensive solution for nonlinear non-Gaussian state estimation that can provide a continuous estimate of the pdf of the system state is developed based on B-splines. This method is capable of modeling any arbitrary pdf of the system state. In addition, the developed B-spline filter is able to provide statistically the same estimation accuracy as the particle filter and without suffering from the degeneracy alike problem due to its continuous nature. Further, the spline filter is also able to handle systems with multiple models, which are justified through simulations.
Keywords: Nonlinear filtering, B-spline, Bayesian filtering, tensor product. I. I NTRODUCTION The problem of nonlinear/non-Gaussian filtering has generated significant interest in the literature. The success of nonlinear/non-Gaussian filtering depends on the accurate representation of the probability density function of the state as well as the likelihood function. In previous works, different approaches have been developed to address the nonlinear filtering problem. In [2], the extended Kalman filter (EKF), which is based on the linearization of the system model, is presented. However, the EKF approximates the system state distribution as Gaussian and, as a result, only the mean and the covariance of the system state are considered in EKF. This approximation degrades the performance of the EKF when the true distribution of the system state is non-Gaussian or highly nonlinear. The unscented transform was embedded into the EKF in [9], which results in the unscented Kalman filter (UKF) but is with limited ability for solving general nonlinear problems. To deal
978-0-9824438-3-5 ©2011 ISIF
with the general nonlinear/non-Gaussian system, Sequential Monte Carlo (SMC)/Markov Chain Monte Carlo (MCMC) approaches are developed in [3, 8, 14], which are based on point mass representation of the pdf of system state, and the resulting filter is the well-known particle filter. A modified implementation of the standard particle filter by using log homotopy is also explored in [6]. The method of spline interpolation has been investigated in literature to solve the nonlinear filtering problem as well. In [4], a spline approach is presented to solve the nonlinear estimation problem of phase demodulation. However, the system model is assumed to be an identical function with zero mean Gaussian noise, and thus the prediction formula is not considered. An approach of using monosplines to solve the nonlinear estimation problem is developed in [18] but with the assumption that the process and measurement noises are additive and Gaussian, which is not always valid in general nonlinear filtering problems. In [12], a spline approach is developed to solve one dimensional nonlinear/non-Gaussian filtering problem, and has been extended to a multidimensional one in [13]. However, the system state transition is not explicitly modeled, and also the explicit state prediction equation is not given but a point mass approach is applied to find the state prediction instead. In [10], tensor product of splines is adopted to represent the system state transition model, but the corresponding analytical state prediction equation is not given, and only one dimensional case is considered. In this paper, a spline filter for standard nonlinear filtering problem is proposed as an effective alternative to particle filters. In the proposed approach, no special assumption of the system model and process/measurement noises is required. The system state transition model is represented by tensor products of splines and the corresponding analytical state prediction equations are derived. In addition, the proposed approach is able to solve multidimensional problems. By combining the spline filter with the Interacting Multiple Mod-
1309
els (IMM) filter, the proposed approach works with multiple model systems. Further, the simulation results show that, unlike the particle filter, the spline filter is insensitive to the the degeneracy problem. This paper is organized as follows. Section II briefly reviews nonlinear Bayesian filtering. Section III provides the background of B-splines. The proposed spline filter and its extension to multidimensional and multiple model systems are presented in Section IV. Simulation results are shown in Section V and conclusions are given in Section VI. II. N ONLINEAR BAYESIAN
FILTERING
The general state space model for discrete-time stochastic systems is of the form xk
= f (xk−1 , νk )
(1)
where xk is the system state at time k, ν is the process noise sequence with known statistics and f is the system transition function, which is possibly nonlinear. The measurement zk at time k is zk
=
h(xk , ωk )
III. B- SPLINES In this section, the background on spline is briefly reviewed and the reader can refer to [5] for further details. A spline is defined as piecewise-polynomial real function, which is of the form (ti+p − ti )[ti , ..., ti+p ](· − x)p−1 +
Bi,p,t (x)
(6)
and Bi,p,t (x) is referred to as the i-th B-spline of order p with knot sequence t. The notation [ti , ..., ti+p ]g stands for the divided difference of order p of g at the sites ti , ..., ti+p and the truncation function (x)+ = max{x, 0}. The placeholder notation is used to indicate that the p-th divided difference of the function (t − x)p−1 of the two variables t and x is to be + taken by fixing x and considering it as a function of t alone. The first order B-spline is given by 1 if ti ≤ x ≤ ti+1 Bi,1,t (x) = 0 otherwise and starting from the first order, higher order B-splines can be constructed using the following recurrence formula Bi,p,t (x)
=
ωi,p Bi,p−i,t (x) + (1 − ωi+1,p )Bi+1,p−i,t (x) (7)
(2) with
where ω is the measurement noise with known statistics and h is the measurement function, which is also possibly nonlinear. In filtering, it is of interest to estimate the probability density function of current state xk in observations of the measurements up to current time. In the Bayesian framework, the sought-after pdf p(xk |z1:k ) is obtained by two steps: 1) prediction, and 2) update. In addition, a prior pdf p(x0 |z0 ) is always assumed to be available for Bayesian filters. The pdf of the predicted state p(xk |z1:k−1 ) can be computed according the total probability theory as follows: p(xk |z1:k−1 ) = p(xk |xk−1 )p(xk−1 |z1:k−1 )dxk−1 (3) where z1:k are the set of measurements up to time k. The transition probability density function p(xk |xk−1 ) is determined by the system model in equation (1). The pdf of the updated state p(xk |z1:k ) is given by p(xk |z1:k )
=
p(zk |xk )p(xk |z1:k−1 ) p(zk |z1:k−1 )
(4)
where p(zk |xk ) is determined by the measurement model in (2), and the denominator p(zk |z1:k−1 ) = p(zk |xk )p(xk |z1:k−1 )dxk (5) is a normalization constant, and it will be shown in Section IV-C that p(zk |z1:k−1 ) is the likelihood used for model probability update in the IMM spline filter.
ωi,p (x)
x − ti ti+p−1 − ti
(8)
Given a basic sequence of B-splines {Bi,p,t }ni=1 and a strictly increasing sequence of data sites {xj }nj=1 , the spline interpolation function fˆ(x) that agrees with function f (x) at all xj is given as fˆ(x)
=
n
αi Bi,p,t (x)
(9)
i=1
where α satisfies n
αi Bi,p,t (xj )
= f (xj ),
for j = 1, ..., n
(10)
i=1
Equation (10) is in fact a linear system of n equations with n unknown αi ’s and the i-th row and j-th column of the coefficient matrix equals Bi,p,t (xj ), which implies that the spline interpolation function can be found by solving a linear system. A. Properties of B-splines 1) Differentiation of B-splines: D
n i=1
αi Bi,p,t (x)
=
(p − 1)
n+1 i=1
αi − αi−1 Bi,p−1,t (x) ti+p−1 − ti (11)
where D(·) denotes the differential operator, and both α0 and αn+1 are zeros.
1310
2) Integral of B-splines: The integral of B-spline over the interval [t1 , x] (t1 ≤ x ≤ ts ) is given by
x
n
t1 i=1
αi Bi,p (y)dy
=
i s−1 i=1
αj (tj+p − tj )p Bi,p+1 (x)
j=1
(12)
B. Tensor Product and Multidimensional Splines Let U and V be linear spaces of functions defined on sets X and Y , respectively, into the reals R. For each u ∈ U and v ∈ V , the function ω(x, y)
u(x)v(y),
(x, y) ∈ X × Y
in literature such as [7] and the references therein, as pointed out in chapter XII of [5], solving the optimal knots placement problem is always computational costly and the gain does not repay the effort in general. Therefore, instead of wasting the computation on the optimal knots selection, it is more convenient to just increase the number of knots with straightforward knots selection, e.g., uniform spaced knots selection. In concern of this argument, in this paper, it is proposed to find the predicted knots according to the system model. That is, τkj
(13)
is referred to as the tensor product of u with v and denoted by u ⊗ v. Further, the set of all finite linear combinations of functions on X × Y of the form u ⊗ v is referred to as the tensor product of U with V and is given by n U ×V αi (ui ⊗ vi ) : αi ∈ R, ui ∈ U, vi ∈ V
p(xk |z1:k−1 ) = p(xk |xk−1 )p(xk−1 |z1:k−1 )dxk−1 = αr,s Br,p,tk (xk )Bs,p,tk−1 (xk−1 )
(14) One-dimensional splines can be extended to multidimensional ones through the use of the tensor product spline constructions. A spline subspace Bij ,pj ,tj (xj ) is defined for each dimension where xj denotes the variable in the j-th dimension. Thus, the spline representation of a multidimensional function f (x1 , ..., xm ) is given as
r,s
·
αi Bi,p,tk−1 (xk−1 )dxk−1
i
=
αr,s Br,p,tk (xk )
r,s
·
f (x1 , ..., xm ) ··· αi1 ,...,im Bi1 ,p1 ,t1 (x1 ) · · · Bim ,pm ,tm (xm ) i1
(18)
where f (·, ·) is function of system transition in equation (1). In this way, it is always assured that the splines cover the region where the probability of system state is significant. Now, the predicted pdf of system state is
i=1
=
j f (τk−1 , νk )
=
αi
Bs,p,tk−1 (xk−1 )Bi,p,tk−1 (xk−1 )dxk−1
i
(19)
im
(15) Similar to the single dimensional case, the construction of the above multidimensional spline polynomials can also be done by solving a corresponding set of linear equations. IV. S PLINE
FILTERING
This section describes the proposed spline implementation of the optimal recursive Bayesian filtering.
where the third equality follows from the property that the order of summation and integration of splines is interchangeable [17]. Let Vi,s = Bs,p,tk−1 (xk−1 )Bi,p,tk−1 (xk−1 )dxk−1 , which are simply integrals of polynomials that can be easily computed, then the predicted pdf of system state can be expressed as p(xk |z1:k−1 ) = αr,s Br,p,tk (xk ) αi Vi,s r
A. Unidimensional Spline Filtering 1) Prediction: Assume that the spline representation of the estimated pdf of system state at time k − 1 is given as p(xk−1 |z1:k−1 ) = αi Bi,p,tk−1 (xk−1 ) (16) i 1 T where tk−1 = {τk−1 , ..., τk−1 } is the set of knots of the splines. The spline representation of the state transition probability p(xk |xk−1 ), which is a two-dimensional function determined by the system model (1), is given as p(xk |xk−1 ) = αr,s Br,p,tk (xk )Bs,p,tk−1 (xk−1 ) (17) r,s
where tk = {τk1 , ..., τkT } is the set of sequence of predicted knots. Although the optimal knots selection problem is studied
=
s
i
αr Br,p,tk (xk )
(20)
r
where the coefficient of the predicted spline representation is given as αr = αr,s αi Vi,s (21) s
i
(C)rs,i
Let = αr,s αi for any fixed r, then a more concise expression of the coefficient is given as αr
= tr(C r V )
(22)
2) Update: After the prediction step, it is straightforward to evaluate the value of p(xk |z1:k−1 ) at any point over the interval [τk1 , τkT ] according to equation (20), which is a continuous estimate of the predicted pdf of the system state.
1311
Then, the interval where p(xk |z1:k−1 ) is significant could be found. After that, from the measurement model (2), the value of the likelihood function p(zk |xk ) can also be evaluated over the same interval, and so does the value of the updated pdf of the system state p(xk |z1:k ) = =
p(zk |xk )p(xk |z1:k−1 ) p(zk |z1:k−1 ) p(zk |xk )p(xk |z1:k−1 ) p(zk |xk )p(xk |z1:k−1 )dxk
j τl,k−1 from the set of knots in each dimension tl,k−1 . For example, the collection of all such sample vectors in the two j i dimensional spline filter is xτ,k−1 = {[τl,k−1 τl,k−1 ] }i,j , and τ,k−1 is T1 × T2 . Then, the predicted the total number of x sample knots xτ,k are found by
xτ,k i,j
(23) In this way, it always ensures that the posterior p(xk |z1:k ) is only evaluated at the interval where it is significant. Once, the significant region is obtained, a simple and effective way of selecting the knots for the posterior is to uniformly distribute the knots over this significant region according to the argument in Section IV-A1. Denote the set of knots of the posterior pdf by tk , then the spline representation of p(xk |z1:k ) p(xk |z1:k ) = αι Bι,p,tk (xk ) (24) ι
can be found by solving a linear system in form of equation (10).
l
The intuition behind equation (28) is to select the knots such that the indices of all the rest Tl − 2 knots are equally spaced. Then, the spline expression of the predicted pdf of system state can be written as
=
The unidimensional spline filter is extended to a multidimensional one in this section. For simplicity, a 2-D example is presented, and the system state at time k is denoted as xk = [x1,k , x2,k ]T . 1) Prediction: Assume that the spline representation of the pdf of system state at time k − 1 is p(xk−1 |z1:k−1 ) = αi,j Bi,p,t1,k−1 (x1,k−1 )
=
where the knots tk−1 consists of n = 2 vectors and the 1 T l-th vector tl,k−1 = {τl,k−1 , ..., τl,k−1 } denotes the set of unidimensional knots of the l-th dimension. In general, the dimension of the state transition function is 2n given that the dimension of the system state is n. For this particular 2-D example, the system state transition function p(xk |xk−1 ) is a 4-D function with spline representation p(xk |xk−1 ) = αr,s,m,n Br,p,t1,k (x1,k )
p(xk |z1:k−1 ) p(xk |xk−1 )p(xk−1 |z1:k−1 )dxk−1 αr,s,m,n Br,p,t1,k (x1,k )Bs,p,t2,k (x2,k ) r,s,m,n
·Bm,p,t1,k−1 (x1,k−1 )Bn,p,t2,k−1 (x2,k−1 ) · αi,j Bi,p,t1,k−1 (x1,k−1 )Bj,p,t2,k−1 (x2,k−1 ) i,j
Define two 4-dimensional matrices V and C, and a 2dimensional matrix D as follows: Vm,n,i,j = Bm,p,t1,k−1 (x1,k−1 )Bn,p,t2,k−1 (x2,k−1 ) ·Bi,p,t1,k−1 (x1,k−1 )Bj,p,t2,k−1 (x2,k−1 )
where tk denotes the knots at time k. In general, the predicted knots selection of multidimensional system is much more challenging than the unidimensional case. Based on the same argument in IV-A1, a suboptimal but computation ease method is used to find the predicted knots for the multidimensional spline. Assume that there are Tl knots in the l-th dimension in tk−1 , then Tl different
(30)
dx1,k−1 dx2,k−1
Cr,s,m,n
r,s,m,n
·Bs,p,t2,k (x2,k )Bm,p,t1,k−1 (x1,k−1 ) (26) ·Bn,p,t2,k−1 (x2,k−1 )
(29)
dx1,k−1 dx2,k−1
i,j
(25)
(27)
Then, to select the predicted knots in each dimension l, first project all the sample vectors xτ,k i,j into the axis of the l-th dimension. In two dimensional case, the projection will result in T1 × T2 points (possibly overlapped points) in each axis ∗T1 ×T2 ∗1 ∗2 and denotes them as {τl,k , τl,k , ..., τl,k } (assume sorted in a nondecreasing order). Second, select the first predicted knot Tl ∗T1 ×T2 1 ∗1 as τl,k = τl,k and the last predicted knot as τl,k = τl,k , φ(i) i and the rest Tl − 2 knots are selected as τl,k = τl,k where φ(i) = 1 + (( Tl ) − 2)/(Tl − 2) · (i − 1) (28)
B. Multidimensional Spline Filtering
·Bj,p,t2,k−1 (x2,k−1 )
= f (xτ,k−1 , νk ) i,j
Di,j
= αr,s,m,n
(31)
=
(32)
αi,j
In addition, for any fixed {r, s} pair, define a 2-dimensional matrix E r,s as r,s Ei,j
=
tr(Cr,s,·,· V·,·,i,j )
(33)
where both Cr,s,·,· and V·,·,i,j are 2-dimensional matrices with row index m and column index n. After some algebra, it can be shown that
l
sample vectors xτ,k−1 can be formed by selecting one knot
1312
p(xk |z1:k−1 ) αr,s Br,p,t1,k (x1,k )Bs,p,t2,k (x2,k ) = r,s
(34)
matched to the j-th model p0j (xk−1 |z1:k−1 ) is
where αr,s is given by αr,s
=
tr(DE r,s )
p0j (xk−1 |z1:k−1 ) M p0j (xk−1 , Mi (k − 1)|z1:k−1 ) =
(35)
2) Update: Similar to the unidimensional case, the pdf of the updated system state
i=1
= p(xk |z1:k ) = =
p(zk |xk )p(xk |z1:k−1 ) p(zk |z1:k−1 ) p(zk |xk )p(xk |z1:k−1 ) p(zk |xk )p(xk |z1:k−1 )dxk
αι,σ Bι,p,t1,k (x1,k )Bσ,p,t2,k (x2,k )
=
(37)
C. IMM Spline Filtering The details of the five steps of standard IMM algorithm are omitted here due to limited space. The readers are referred to [2] for further details. An IMM spline filter is to be described in the rest part of this section and the notations are kept identical as those in [2]. Similar to the IMM in [2], it is assumed that there are M spline filters matched to the M system models. Further, the estimated pdf of the system state and the likelihood function from the i-th spline filter are denoted as pi (x) and pi (z|x), respectively, and keeping other notations the same as those of the standard IMM filter. Before giving the general steps of the IMM spline filter, it is worth noting that in [2] the computation of the likelihood Λj (k) assumes that the distribution of the innovation νj (k) is approximately normal. However, this assumption is not valid for general nonlinear system where the spline filter works. The equation for calculating Λj (k) in the general case is given by p(z(k)|Mj (k), z1:k−1 ) = pj (zk |xk )pj (xk |z1:k−1 )dxk
M
μi|j (k − 1|k − 1)
m
i=1
(36)
ι,σ
Λj (k)
pi (xk−1 |z1:k−1 )μi|j (k − 1|k − 1)
i=1
can be found after evaluating its value over the region defined by tk , i.e., the 2-dimensional region covered by the splines, and denoted as p(xk |z1:k ) =
M
αim Bim,p,tk−1 (xk−1 ) (39)
for j = 1, ..., M
3. Model-matched filtering: The mixed initial pdf p0j (xk−1 |z1:k−1 ) is used as input to the spline filter matched to the j-th model, which uses z(k) to yield pj (xk |z1:k ). The corresponding likelihood Λj (k) is given by using equation (38). 4. Model probabilities update: This steps is also exactly the same as that of the standard IMM filter. 5. Probability density function combination: The combination of the model-conditioned pdf of the system state is given by p(xk |z1:k )
M
=
p(xk , Mj (k)|z1:k )
j=1 M
=
pj (xk |z1:k )μj (k)
j=1 M
=
μj (k)
m
j=1
αjm Bjm,p,tk−1 (xk )
for j = 1, ..., M
(40)
V. S IMULATIONS A simulated bearing-only scenario will be used throughout this section. As shown in Figure 1, the platform is moving along the line y = yp (m) while the target is moving along the x-axis. The x-position of the platform is xp (k) = 4k at time k. The measurement provided by the platform is the angle between the x-axis and line-of-sight from the platform to the target. This scenario was previously used for comparing different nonlinear filters [2].
(38) A. Unidimensional Spline Filter
which is exactly the denominator of equation (23). Now, the steps of the IMM spline filter are summarized as follows: 1. Calculation of the mixing probabilities: This step is exactly the same as that of the standard IMM filter. 2. Mixing: The mixed initial pdf of the system state for the spline filter
To justify the effectiveness of the spline filter, the target state is assumed to be the x-position of the target, and the system model is xk
=
xk−1 + 1 + νk
(41)
and the measurement model is
1313
zk
=
arctan
yp (k) + ωk xk − 4k
(42)
3
SP PF (N = 100) PF (N = 1000) PCRLB
Platform 2.5
RMSE (m)
Target
p
2
1.5
1
0.5 0
Platform and target motion.
10 Time (s)
15
20
Figure 2. RMSE comparison of spline filter and particle filter (σω = 1deg).
where both νk and ωk are zero mean Gaussian random variables with standard deviations 0.5m and 1deg, respectively. The initial x-position of the target is 80m and yp (k) = 50m. The Root-Mean-Square Errors (RMSEs) of 100 Monte Carlo runs of the spline filter of order 3 with 10 knots, a particle filter with 100 particles and a particle filter with 1000 particles are shown in Figure 2 along with the Posterior Cramer-Rao Lower Bound (PCRLB) [15, 16]. For both particle filters, the resampling procedure is activated when the effective sample size [1] Nef f is less than 80%. Define the estimation error as x ˜(k) x(k) − xˆ(k) and denote the corresponding PCRLB as J −1 (k), then it has been shown in [2] that the normalized estimation error squared (NEES) for x, defined as x x ˜ J x ˜ is (approximately) χ2 distributed. The NEES of the spline filter and two particle filters are drawn in Figure 3 along with the 95% confidence-region of the χ2 distribution. From Figure 3, it can be observed that 1) the spline filter using 10 knots provides better performance than the particle filter using 100 knots in this scenario; 2) the RMSE of the spline filter is the same as the particle filter using 1000 particles and the NEESs of both filters fall inside the 95% confidenceregion which implies that their performances are statistically the same1 . B. Degeneracy Problem
1.4
1.2 1.1 1 0.9 0.8 0.7 0
5
10 Time (s)
15
20
Figure 3. NEES comparison of spline filter and particle filter (σω = 1deg).
spline filter is able to provide efficient results, i.e., its NEES falls inside the 95% confidence region, without increasing the number of knots. C. Multidimensional Spline Filter In this subsection, it is assumed that the system state xk consists of the x-position ηk and velocity ξk of the target, i.e.,
To illustrate the degeneracy alleviation ability of the proposed spline filter, the standard deviation of the measurement noise is intentionally reduced to a very small value 0.05deg in the above scenario so that the variance of the weights of particles increases rapidly for particle filters. The parameters of the three filters remain unchanged. Figure 4 and Figure 5 show the corresponding RMSEs and NEESs, respectively, over 100 Monte Carlo runs. It can be observed in Figure 3 and Figure 5 that the particle filter has to increase the number of particles from 100 to 1000 to overcome the particle degeneracy caused by the reduction of measurement noise. However, by comparing Figure 3 and Figure 5, it can be shown that the
0.22
SP PF (N = 100) PF (N = 1000) PCRLB
0.2 0.18
RMSE (m)
0.16 0.14 0.12 0.1 0.08 0.06 0.04 0
1 In
our current implementation, the particle filter using 1000 particles is faster than the spline filter with 10 knots in MATLAB. The rigorous analysis of the computation load of the spline filter and the running time comparison with particle filters are remained as a future work.
SP PF (N = 100) PF (N = 1000) 95% confidence region
1.3
NEES
Figure 1.
5
Figure 4. 0.05deg).
1314
5
10 Time (s)
15
20
RMSE comparison of spline filter and particle filter (σω =
4.5
SP PF (N = 100) PF (N = 1000) 95% confidence region
4 3.5
0.8 0.6
Probability
NEES
3 2.5
0.4 0.2
2
0 4
1.5
100
2
1
95 90
0
0.5 0
Figure 5. 0.05deg).
5
10 Time (s)
15
20
NEES comparison of spline filter and particle filter (σω =
85 −2
Velocity (m/s)
Figure 7.
80
Position (m)
Predicted pdf of the state.
0.25
0.25
0.2
Probability
Probability
0.2 0.15 0.1
0.15 0.1 0.05
0.05
0 4
0 4
100
2
95
95
2
0
80
−2
Velocity (m/s)
85 −2
Position (m)
Updated pdf of the state.
Prior pdf of the state.
xk = [ηk ξk ] . The system model is given as
1 1 0.5 xk−1 + νk xk = 0 1 1
and xk (43)
= arctan
=
0.5 1 1 −0.4 ν xk−1 + (k − 1) + 0.01 2,k 0 1 0 (46)
and the measurement model is
and the measurement model is zk
80
Position (m)
Figure 8. Figure 6.
85
90
0 Velocity (m/s)
90
yp (k) + ωk ηk − 4k
zk
(44)
where both νk and ωk are zero mean Gaussian random variables with standard deviation 0.05m and 1deg, respectively. The initial target state is [80m 1m/s] and yp (k) = 20m. A spline filter of order 3 is used to track the target, and 10 knots are used for the position dimension and 4 knots for the velocity dimension. Figure 6∼8 show the prior pdf, predicted pdf and updated pdf of the system state at time t = 10s. D. IMM Spline Filter In this subsection, a system with two models is applied to the target. In the second model, a time-varying control term is added. Specifically, the system models are
1 1 0.5 xk = x ν + (45) 0 1 k−1 0.01 1,k
= arctan
yp (k) + ωk ηk − 4k
(47)
where ν1,k , ν2,k and ωk are all zero mean Gaussian random variables with standard deviation 0.8m, 0.8m and 1deg, respectively. The initial target state is [30m 5m/s] and the target starts with model 1 and switches to model 2 at time t = 15s, and yp (k) = 20m. A spline filter of order 3 is used to track the target, and 10 knots are used for the position dimension and 4 knots for the velocity dimension. The estimated model probability is shown in Figure 9, and the target position estimate and velocity estimate are shown in Figure 10 and Figure 11, respectively. From Figure 9∼11, it can be shown that the proposed spline filter is able to track a maneuvering target and detect target model switching. VI. C ONCLUSIONS In this paper, a spline implementation of the Bayesian filter, which provides continuous estimate of the pdf of sys-
1315
1
same as that of the particle filter, thus the spline filter could be used as an effective alternative to the particle filer. In addition, the developed spline filter is free from the degeneracy alike problem due to its continuous nature. Further, the spline filter is able to handle systems with multiple models, which is also justified by the simulation result.
Truth SP
Mode Probability
0.8
0.6
0.4
R EFERENCES
0.2
0 0
5
10
15
20
25
Time (s)
Figure 9.
Mode probability.
100 90
Position (m)
80 70 60 50 40 30 0
Truth SP 5
10
15
20
25
Time (s)
Figure 10.
Position estimate and the truth.
tem state, is developed for multidimensional nonlinear/nonGaussian systems. The analytical solution of the spline filter is developed by representing the system transition model using tensor products of splines. By moving the knots of the spline according to the system transition model, it ensures that the splines always cover the region where the probability of the system state is significant. Simulation results show that the estimation performance of the spline filter is statistically the
10
Truth SP
9
Velocity (m/s)
8 7
[1] Arulampalam, M. S., Maskell, S., Gordon, N., and Clapp, T., “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,” IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 174–188, Feb. 2002. [2] Bar-Shalom, Y., Li, X., and Kirubarajan, T., Estimation with Applications to Tracking and Navigation. John Wiley, New York, 2001. [3] Berzuini, C., Best, N. G., Gilks, W. R., and Larizza, C., “Dynamical conditional independence models and Markov Chain Monte Carlo methods,” Journal of the American Statistical Association, vol. 92, no. 440, pp. 1403–1412, Dec. 1997. [4] Bucy, R. S., and Youssef, H. M., “Nonlinear filter representation via spline functions,” Proceedings of the 5th Symposium on Nonlinear Estimation, pp. 51–60, 1974. [5] De Boor, C., A Practical Guide to Splines. Springer-Verlag, New York, 2007. [6] Daum, F., and Huang, J., “Particle flow for nonlinear filters with loghomotopy,” Proceedings of SPIE, vol. 6969, 2008. [7] Goldenthal, R., and Bercovier, M., “Spline curve approximation and design by optimal control over the knots,” Computing, vol. 72, no. 1, pp. 53–64, Apr. 2004. [8] Gordon, N. J., Salmond, D., and Smith, A., “Novel approach to nonlinear/non-Gaussian Bayesian state estimation,” Radar and Signal Processing, IEE Proceedings F, vol. 140, no. 2, pp. 107–113, Aug. 2002. [9] Julier, S. J., and Uhlmann, J. K., “A new extension of the Kalman filter to nonlinear systems,” Proceedings of AeroSense, Simulation and Controls, vol. 3, 1997. [10] Kocherry, D. L., Tharmarasa, R., Lang, T., and Kirubarajan, T., “Spline filter for target tracking,” Proceedings of SPIE, vol. 7698, 2010. [11] Prautzsch, H., Boehm, W., and Paluszny, M., B´ezier and B-spline techniques. Springer-Verlag, 2002. [12] Punithakumar, K., and Kirubarajan, T., “Spline filter for nonlinear/nonGaussian Bayesian tracking,” Proceedings of SPIE, vol. 6699, 2007. [13] Punithakumar, K., McDonald, M., and Kirubarajan, T., “Spline filter for nonlinear/non-Gaussian Bayesian tracking,” Proceedings of SPIE, vol. 6969, 2008. [14] Septier, F., Pang, S. K., Carmi, A., and Godsill, S., “On MCMCBased particle methods for Bayesian filtering: Application to multitarget tracking,” Computational Advances in Multi-Sensor Adaptive Processing (CAMSAP), 2009 3rd IEEE International Workshop on, Aruba, Dutch Antilles, Feb. 2009. [15] Tharmarasa, R., Kirubarajan, T., Hernandez, M. L., and Sinha, A., “PCRLB-based multisensor array management for multitarget tracking,” IEEE Transactions on Aerospace and Electronic Systems, vol. 43, no. 2, pp. 539–555, Aug. 2007. [16] Van Trees, H., Detection, Estimation and Modulation Theory. vol. I, John Wiley, New York, 1968. [17] Vermeulen, A. H., Bartels, R. H., and Heppler, G. R., “Integrating products of B-splines,” SIAM Journal on Scientific and Statistical Computing, vol. 13, no. 4, pp. 1025–1038, Jul. 1992. [18] Wang, A. H., and Klein, R. L., “Implementation of non-linear estimators using monospline,” Proceedings of the 15th IEEE Conference on Decision and Control, vol. 74, pp. 1305–1307, 1976.
6 5 4 3 2 0
5
10
15
20
25
Time (s)
Figure 11.
Velocity estimate and the truth.
1316
A Spline Filter for Multidimensional Nonlinear State Estimation Xiaofan He
Ratnasingham Tharmarasa
Donna Lynn Kocherry
Dept. of ECE McMaster University Hamilton, Canada. [email protected]
Dept. of ECE McMaster University Hamilton, Canada. [email protected]
Dept. of ECE McMaster University Hamilton, Canada. [email protected]
Bhashyam Balaji
Thiagalingam Kirubarajan
Radar Systems Section Defence Research and Development Canada Ottawa, Canada. [email protected]
Dept. of ECE McMaster University Hamilton, Canada. [email protected]
Abstract—The problem of nonlinear/non-Gaussian filtering has generated significant interest in the literature. Sequential Monte Carlo (SMC)/Markov Chain Monte Carlo (MCMC) approaches are the most commonly used. The success of nonlinear/nonGaussian filtering depends on the accurate representation of the pdf of the system state as well as the likelihood function. However, the commonly used Monte Carlo approaches only provide weighted samples at discrete points in the state space. In this paper, a comprehensive solution for nonlinear non-Gaussian state estimation that can provide a continuous estimate of the pdf of the system state is developed based on B-splines. This method is capable of modeling any arbitrary pdf of the system state. In addition, the developed B-spline filter is able to provide statistically the same estimation accuracy as the particle filter and without suffering from the degeneracy alike problem due to its continuous nature. Further, the spline filter is also able to handle systems with multiple models, which are justified through simulations.
Keywords: Nonlinear filtering, B-spline, Bayesian filtering, tensor product. I. I NTRODUCTION The problem of nonlinear/non-Gaussian filtering has generated significant interest in the literature. The success of nonlinear/non-Gaussian filtering depends on the accurate representation of the probability density function of the state as well as the likelihood function. In previous works, different approaches have been developed to address the nonlinear filtering problem. In [2], the extended Kalman filter (EKF), which is based on the linearization of the system model, is presented. However, the EKF approximates the system state distribution as Gaussian and, as a result, only the mean and the covariance of the system state are considered in EKF. This approximation degrades the performance of the EKF when the true distribution of the system state is non-Gaussian or highly nonlinear. The unscented transform was embedded into the EKF in [9], which results in the unscented Kalman filter (UKF) but is with limited ability for solving general nonlinear problems. To deal
978-0-9824438-3-5 ©2011 ISIF
with the general nonlinear/non-Gaussian system, Sequential Monte Carlo (SMC)/Markov Chain Monte Carlo (MCMC) approaches are developed in [3, 8, 14], which are based on point mass representation of the pdf of system state, and the resulting filter is the well-known particle filter. A modified implementation of the standard particle filter by using log homotopy is also explored in [6]. The method of spline interpolation has been investigated in literature to solve the nonlinear filtering problem as well. In [4], a spline approach is presented to solve the nonlinear estimation problem of phase demodulation. However, the system model is assumed to be an identical function with zero mean Gaussian noise, and thus the prediction formula is not considered. An approach of using monosplines to solve the nonlinear estimation problem is developed in [18] but with the assumption that the process and measurement noises are additive and Gaussian, which is not always valid in general nonlinear filtering problems. In [12], a spline approach is developed to solve one dimensional nonlinear/non-Gaussian filtering problem, and has been extended to a multidimensional one in [13]. However, the system state transition is not explicitly modeled, and also the explicit state prediction equation is not given but a point mass approach is applied to find the state prediction instead. In [10], tensor product of splines is adopted to represent the system state transition model, but the corresponding analytical state prediction equation is not given, and only one dimensional case is considered. In this paper, a spline filter for standard nonlinear filtering problem is proposed as an effective alternative to particle filters. In the proposed approach, no special assumption of the system model and process/measurement noises is required. The system state transition model is represented by tensor products of splines and the corresponding analytical state prediction equations are derived. In addition, the proposed approach is able to solve multidimensional problems. By combining the spline filter with the Interacting Multiple Mod-
1309
els (IMM) filter, the proposed approach works with multiple model systems. Further, the simulation results show that, unlike the particle filter, the spline filter is insensitive to the the degeneracy problem. This paper is organized as follows. Section II briefly reviews nonlinear Bayesian filtering. Section III provides the background of B-splines. The proposed spline filter and its extension to multidimensional and multiple model systems are presented in Section IV. Simulation results are shown in Section V and conclusions are given in Section VI. II. N ONLINEAR BAYESIAN
FILTERING
The general state space model for discrete-time stochastic systems is of the form xk
= f (xk−1 , νk )
(1)
where xk is the system state at time k, ν is the process noise sequence with known statistics and f is the system transition function, which is possibly nonlinear. The measurement zk at time k is zk
=
h(xk , ωk )
III. B- SPLINES In this section, the background on spline is briefly reviewed and the reader can refer to [5] for further details. A spline is defined as piecewise-polynomial real function, which is of the form (ti+p − ti )[ti , ..., ti+p ](· − x)p−1 +
Bi,p,t (x)
(6)
and Bi,p,t (x) is referred to as the i-th B-spline of order p with knot sequence t. The notation [ti , ..., ti+p ]g stands for the divided difference of order p of g at the sites ti , ..., ti+p and the truncation function (x)+ = max{x, 0}. The placeholder notation is used to indicate that the p-th divided difference of the function (t − x)p−1 of the two variables t and x is to be + taken by fixing x and considering it as a function of t alone. The first order B-spline is given by 1 if ti ≤ x ≤ ti+1 Bi,1,t (x) = 0 otherwise and starting from the first order, higher order B-splines can be constructed using the following recurrence formula Bi,p,t (x)
=
ωi,p Bi,p−i,t (x) + (1 − ωi+1,p )Bi+1,p−i,t (x) (7)
(2) with
where ω is the measurement noise with known statistics and h is the measurement function, which is also possibly nonlinear. In filtering, it is of interest to estimate the probability density function of current state xk in observations of the measurements up to current time. In the Bayesian framework, the sought-after pdf p(xk |z1:k ) is obtained by two steps: 1) prediction, and 2) update. In addition, a prior pdf p(x0 |z0 ) is always assumed to be available for Bayesian filters. The pdf of the predicted state p(xk |z1:k−1 ) can be computed according the total probability theory as follows: p(xk |z1:k−1 ) = p(xk |xk−1 )p(xk−1 |z1:k−1 )dxk−1 (3) where z1:k are the set of measurements up to time k. The transition probability density function p(xk |xk−1 ) is determined by the system model in equation (1). The pdf of the updated state p(xk |z1:k ) is given by p(xk |z1:k )
=
p(zk |xk )p(xk |z1:k−1 ) p(zk |z1:k−1 )
(4)
where p(zk |xk ) is determined by the measurement model in (2), and the denominator p(zk |z1:k−1 ) = p(zk |xk )p(xk |z1:k−1 )dxk (5) is a normalization constant, and it will be shown in Section IV-C that p(zk |z1:k−1 ) is the likelihood used for model probability update in the IMM spline filter.
ωi,p (x)
x − ti ti+p−1 − ti
(8)
Given a basic sequence of B-splines {Bi,p,t }ni=1 and a strictly increasing sequence of data sites {xj }nj=1 , the spline interpolation function fˆ(x) that agrees with function f (x) at all xj is given as fˆ(x)
=
n
αi Bi,p,t (x)
(9)
i=1
where α satisfies n
αi Bi,p,t (xj )
= f (xj ),
for j = 1, ..., n
(10)
i=1
Equation (10) is in fact a linear system of n equations with n unknown αi ’s and the i-th row and j-th column of the coefficient matrix equals Bi,p,t (xj ), which implies that the spline interpolation function can be found by solving a linear system. A. Properties of B-splines 1) Differentiation of B-splines: D
n i=1
αi Bi,p,t (x)
=
(p − 1)
n+1 i=1
αi − αi−1 Bi,p−1,t (x) ti+p−1 − ti (11)
where D(·) denotes the differential operator, and both α0 and αn+1 are zeros.
1310
2) Integral of B-splines: The integral of B-spline over the interval [t1 , x] (t1 ≤ x ≤ ts ) is given by
x
n
t1 i=1
αi Bi,p (y)dy
=
i s−1 i=1
αj (tj+p − tj )p Bi,p+1 (x)
j=1
(12)
B. Tensor Product and Multidimensional Splines Let U and V be linear spaces of functions defined on sets X and Y , respectively, into the reals R. For each u ∈ U and v ∈ V , the function ω(x, y)
u(x)v(y),
(x, y) ∈ X × Y
in literature such as [7] and the references therein, as pointed out in chapter XII of [5], solving the optimal knots placement problem is always computational costly and the gain does not repay the effort in general. Therefore, instead of wasting the computation on the optimal knots selection, it is more convenient to just increase the number of knots with straightforward knots selection, e.g., uniform spaced knots selection. In concern of this argument, in this paper, it is proposed to find the predicted knots according to the system model. That is, τkj
(13)
is referred to as the tensor product of u with v and denoted by u ⊗ v. Further, the set of all finite linear combinations of functions on X × Y of the form u ⊗ v is referred to as the tensor product of U with V and is given by n U ×V αi (ui ⊗ vi ) : αi ∈ R, ui ∈ U, vi ∈ V
p(xk |z1:k−1 ) = p(xk |xk−1 )p(xk−1 |z1:k−1 )dxk−1 = αr,s Br,p,tk (xk )Bs,p,tk−1 (xk−1 )
(14) One-dimensional splines can be extended to multidimensional ones through the use of the tensor product spline constructions. A spline subspace Bij ,pj ,tj (xj ) is defined for each dimension where xj denotes the variable in the j-th dimension. Thus, the spline representation of a multidimensional function f (x1 , ..., xm ) is given as
r,s
·
αi Bi,p,tk−1 (xk−1 )dxk−1
i
=
αr,s Br,p,tk (xk )
r,s
·
f (x1 , ..., xm ) ··· αi1 ,...,im Bi1 ,p1 ,t1 (x1 ) · · · Bim ,pm ,tm (xm ) i1
(18)
where f (·, ·) is function of system transition in equation (1). In this way, it is always assured that the splines cover the region where the probability of system state is significant. Now, the predicted pdf of system state is
i=1
=
j f (τk−1 , νk )
=
αi
Bs,p,tk−1 (xk−1 )Bi,p,tk−1 (xk−1 )dxk−1
i
(19)
im
(15) Similar to the single dimensional case, the construction of the above multidimensional spline polynomials can also be done by solving a corresponding set of linear equations. IV. S PLINE
FILTERING
This section describes the proposed spline implementation of the optimal recursive Bayesian filtering.
where the third equality follows from the property that the order of summation and integration of splines is interchangeable [17]. Let Vi,s = Bs,p,tk−1 (xk−1 )Bi,p,tk−1 (xk−1 )dxk−1 , which are simply integrals of polynomials that can be easily computed, then the predicted pdf of system state can be expressed as p(xk |z1:k−1 ) = αr,s Br,p,tk (xk ) αi Vi,s r
A. Unidimensional Spline Filtering 1) Prediction: Assume that the spline representation of the estimated pdf of system state at time k − 1 is given as p(xk−1 |z1:k−1 ) = αi Bi,p,tk−1 (xk−1 ) (16) i 1 T where tk−1 = {τk−1 , ..., τk−1 } is the set of knots of the splines. The spline representation of the state transition probability p(xk |xk−1 ), which is a two-dimensional function determined by the system model (1), is given as p(xk |xk−1 ) = αr,s Br,p,tk (xk )Bs,p,tk−1 (xk−1 ) (17) r,s
where tk = {τk1 , ..., τkT } is the set of sequence of predicted knots. Although the optimal knots selection problem is studied
=
s
i
αr Br,p,tk (xk )
(20)
r
where the coefficient of the predicted spline representation is given as αr = αr,s αi Vi,s (21) s
i
(C)rs,i
Let = αr,s αi for any fixed r, then a more concise expression of the coefficient is given as αr
= tr(C r V )
(22)
2) Update: After the prediction step, it is straightforward to evaluate the value of p(xk |z1:k−1 ) at any point over the interval [τk1 , τkT ] according to equation (20), which is a continuous estimate of the predicted pdf of the system state.
1311
Then, the interval where p(xk |z1:k−1 ) is significant could be found. After that, from the measurement model (2), the value of the likelihood function p(zk |xk ) can also be evaluated over the same interval, and so does the value of the updated pdf of the system state p(xk |z1:k ) = =
p(zk |xk )p(xk |z1:k−1 ) p(zk |z1:k−1 ) p(zk |xk )p(xk |z1:k−1 ) p(zk |xk )p(xk |z1:k−1 )dxk
j τl,k−1 from the set of knots in each dimension tl,k−1 . For example, the collection of all such sample vectors in the two j i dimensional spline filter is xτ,k−1 = {[τl,k−1 τl,k−1 ] }i,j , and τ,k−1 is T1 × T2 . Then, the predicted the total number of x sample knots xτ,k are found by
xτ,k i,j
(23) In this way, it always ensures that the posterior p(xk |z1:k ) is only evaluated at the interval where it is significant. Once, the significant region is obtained, a simple and effective way of selecting the knots for the posterior is to uniformly distribute the knots over this significant region according to the argument in Section IV-A1. Denote the set of knots of the posterior pdf by tk , then the spline representation of p(xk |z1:k ) p(xk |z1:k ) = αι Bι,p,tk (xk ) (24) ι
can be found by solving a linear system in form of equation (10).
l
The intuition behind equation (28) is to select the knots such that the indices of all the rest Tl − 2 knots are equally spaced. Then, the spline expression of the predicted pdf of system state can be written as
=
The unidimensional spline filter is extended to a multidimensional one in this section. For simplicity, a 2-D example is presented, and the system state at time k is denoted as xk = [x1,k , x2,k ]T . 1) Prediction: Assume that the spline representation of the pdf of system state at time k − 1 is p(xk−1 |z1:k−1 ) = αi,j Bi,p,t1,k−1 (x1,k−1 )
=
where the knots tk−1 consists of n = 2 vectors and the 1 T l-th vector tl,k−1 = {τl,k−1 , ..., τl,k−1 } denotes the set of unidimensional knots of the l-th dimension. In general, the dimension of the state transition function is 2n given that the dimension of the system state is n. For this particular 2-D example, the system state transition function p(xk |xk−1 ) is a 4-D function with spline representation p(xk |xk−1 ) = αr,s,m,n Br,p,t1,k (x1,k )
p(xk |z1:k−1 ) p(xk |xk−1 )p(xk−1 |z1:k−1 )dxk−1 αr,s,m,n Br,p,t1,k (x1,k )Bs,p,t2,k (x2,k ) r,s,m,n
·Bm,p,t1,k−1 (x1,k−1 )Bn,p,t2,k−1 (x2,k−1 ) · αi,j Bi,p,t1,k−1 (x1,k−1 )Bj,p,t2,k−1 (x2,k−1 ) i,j
Define two 4-dimensional matrices V and C, and a 2dimensional matrix D as follows: Vm,n,i,j = Bm,p,t1,k−1 (x1,k−1 )Bn,p,t2,k−1 (x2,k−1 ) ·Bi,p,t1,k−1 (x1,k−1 )Bj,p,t2,k−1 (x2,k−1 )
where tk denotes the knots at time k. In general, the predicted knots selection of multidimensional system is much more challenging than the unidimensional case. Based on the same argument in IV-A1, a suboptimal but computation ease method is used to find the predicted knots for the multidimensional spline. Assume that there are Tl knots in the l-th dimension in tk−1 , then Tl different
(30)
dx1,k−1 dx2,k−1
Cr,s,m,n
r,s,m,n
·Bs,p,t2,k (x2,k )Bm,p,t1,k−1 (x1,k−1 ) (26) ·Bn,p,t2,k−1 (x2,k−1 )
(29)
dx1,k−1 dx2,k−1
i,j
(25)
(27)
Then, to select the predicted knots in each dimension l, first project all the sample vectors xτ,k i,j into the axis of the l-th dimension. In two dimensional case, the projection will result in T1 × T2 points (possibly overlapped points) in each axis ∗T1 ×T2 ∗1 ∗2 and denotes them as {τl,k , τl,k , ..., τl,k } (assume sorted in a nondecreasing order). Second, select the first predicted knot Tl ∗T1 ×T2 1 ∗1 as τl,k = τl,k and the last predicted knot as τl,k = τl,k , φ(i) i and the rest Tl − 2 knots are selected as τl,k = τl,k where φ(i) = 1 + (( Tl ) − 2)/(Tl − 2) · (i − 1) (28)
B. Multidimensional Spline Filtering
·Bj,p,t2,k−1 (x2,k−1 )
= f (xτ,k−1 , νk ) i,j
Di,j
= αr,s,m,n
(31)
=
(32)
αi,j
In addition, for any fixed {r, s} pair, define a 2-dimensional matrix E r,s as r,s Ei,j
=
tr(Cr,s,·,· V·,·,i,j )
(33)
where both Cr,s,·,· and V·,·,i,j are 2-dimensional matrices with row index m and column index n. After some algebra, it can be shown that
l
sample vectors xτ,k−1 can be formed by selecting one knot
1312
p(xk |z1:k−1 ) αr,s Br,p,t1,k (x1,k )Bs,p,t2,k (x2,k ) = r,s
(34)
matched to the j-th model p0j (xk−1 |z1:k−1 ) is
where αr,s is given by αr,s
=
tr(DE r,s )
p0j (xk−1 |z1:k−1 ) M p0j (xk−1 , Mi (k − 1)|z1:k−1 ) =
(35)
2) Update: Similar to the unidimensional case, the pdf of the updated system state
i=1
= p(xk |z1:k ) = =
p(zk |xk )p(xk |z1:k−1 ) p(zk |z1:k−1 ) p(zk |xk )p(xk |z1:k−1 ) p(zk |xk )p(xk |z1:k−1 )dxk
αι,σ Bι,p,t1,k (x1,k )Bσ,p,t2,k (x2,k )
=
(37)
C. IMM Spline Filtering The details of the five steps of standard IMM algorithm are omitted here due to limited space. The readers are referred to [2] for further details. An IMM spline filter is to be described in the rest part of this section and the notations are kept identical as those in [2]. Similar to the IMM in [2], it is assumed that there are M spline filters matched to the M system models. Further, the estimated pdf of the system state and the likelihood function from the i-th spline filter are denoted as pi (x) and pi (z|x), respectively, and keeping other notations the same as those of the standard IMM filter. Before giving the general steps of the IMM spline filter, it is worth noting that in [2] the computation of the likelihood Λj (k) assumes that the distribution of the innovation νj (k) is approximately normal. However, this assumption is not valid for general nonlinear system where the spline filter works. The equation for calculating Λj (k) in the general case is given by p(z(k)|Mj (k), z1:k−1 ) = pj (zk |xk )pj (xk |z1:k−1 )dxk
M
μi|j (k − 1|k − 1)
m
i=1
(36)
ι,σ
Λj (k)
pi (xk−1 |z1:k−1 )μi|j (k − 1|k − 1)
i=1
can be found after evaluating its value over the region defined by tk , i.e., the 2-dimensional region covered by the splines, and denoted as p(xk |z1:k ) =
M
αim Bim,p,tk−1 (xk−1 ) (39)
for j = 1, ..., M
3. Model-matched filtering: The mixed initial pdf p0j (xk−1 |z1:k−1 ) is used as input to the spline filter matched to the j-th model, which uses z(k) to yield pj (xk |z1:k ). The corresponding likelihood Λj (k) is given by using equation (38). 4. Model probabilities update: This steps is also exactly the same as that of the standard IMM filter. 5. Probability density function combination: The combination of the model-conditioned pdf of the system state is given by p(xk |z1:k )
M
=
p(xk , Mj (k)|z1:k )
j=1 M
=
pj (xk |z1:k )μj (k)
j=1 M
=
μj (k)
m
j=1
αjm Bjm,p,tk−1 (xk )
for j = 1, ..., M
(40)
V. S IMULATIONS A simulated bearing-only scenario will be used throughout this section. As shown in Figure 1, the platform is moving along the line y = yp (m) while the target is moving along the x-axis. The x-position of the platform is xp (k) = 4k at time k. The measurement provided by the platform is the angle between the x-axis and line-of-sight from the platform to the target. This scenario was previously used for comparing different nonlinear filters [2].
(38) A. Unidimensional Spline Filter
which is exactly the denominator of equation (23). Now, the steps of the IMM spline filter are summarized as follows: 1. Calculation of the mixing probabilities: This step is exactly the same as that of the standard IMM filter. 2. Mixing: The mixed initial pdf of the system state for the spline filter
To justify the effectiveness of the spline filter, the target state is assumed to be the x-position of the target, and the system model is xk
=
xk−1 + 1 + νk
(41)
and the measurement model is
1313
zk
=
arctan
yp (k) + ωk xk − 4k
(42)
3
SP PF (N = 100) PF (N = 1000) PCRLB
Platform 2.5
RMSE (m)
Target
p
2
1.5
1
0.5 0
Platform and target motion.
10 Time (s)
15
20
Figure 2. RMSE comparison of spline filter and particle filter (σω = 1deg).
where both νk and ωk are zero mean Gaussian random variables with standard deviations 0.5m and 1deg, respectively. The initial x-position of the target is 80m and yp (k) = 50m. The Root-Mean-Square Errors (RMSEs) of 100 Monte Carlo runs of the spline filter of order 3 with 10 knots, a particle filter with 100 particles and a particle filter with 1000 particles are shown in Figure 2 along with the Posterior Cramer-Rao Lower Bound (PCRLB) [15, 16]. For both particle filters, the resampling procedure is activated when the effective sample size [1] Nef f is less than 80%. Define the estimation error as x ˜(k) x(k) − xˆ(k) and denote the corresponding PCRLB as J −1 (k), then it has been shown in [2] that the normalized estimation error squared (NEES) for x, defined as x x ˜ J x ˜ is (approximately) χ2 distributed. The NEES of the spline filter and two particle filters are drawn in Figure 3 along with the 95% confidence-region of the χ2 distribution. From Figure 3, it can be observed that 1) the spline filter using 10 knots provides better performance than the particle filter using 100 knots in this scenario; 2) the RMSE of the spline filter is the same as the particle filter using 1000 particles and the NEESs of both filters fall inside the 95% confidenceregion which implies that their performances are statistically the same1 . B. Degeneracy Problem
1.4
1.2 1.1 1 0.9 0.8 0.7 0
5
10 Time (s)
15
20
Figure 3. NEES comparison of spline filter and particle filter (σω = 1deg).
spline filter is able to provide efficient results, i.e., its NEES falls inside the 95% confidence region, without increasing the number of knots. C. Multidimensional Spline Filter In this subsection, it is assumed that the system state xk consists of the x-position ηk and velocity ξk of the target, i.e.,
To illustrate the degeneracy alleviation ability of the proposed spline filter, the standard deviation of the measurement noise is intentionally reduced to a very small value 0.05deg in the above scenario so that the variance of the weights of particles increases rapidly for particle filters. The parameters of the three filters remain unchanged. Figure 4 and Figure 5 show the corresponding RMSEs and NEESs, respectively, over 100 Monte Carlo runs. It can be observed in Figure 3 and Figure 5 that the particle filter has to increase the number of particles from 100 to 1000 to overcome the particle degeneracy caused by the reduction of measurement noise. However, by comparing Figure 3 and Figure 5, it can be shown that the
0.22
SP PF (N = 100) PF (N = 1000) PCRLB
0.2 0.18
RMSE (m)
0.16 0.14 0.12 0.1 0.08 0.06 0.04 0
1 In
our current implementation, the particle filter using 1000 particles is faster than the spline filter with 10 knots in MATLAB. The rigorous analysis of the computation load of the spline filter and the running time comparison with particle filters are remained as a future work.
SP PF (N = 100) PF (N = 1000) 95% confidence region
1.3
NEES
Figure 1.
5
Figure 4. 0.05deg).
1314
5
10 Time (s)
15
20
RMSE comparison of spline filter and particle filter (σω =
4.5
SP PF (N = 100) PF (N = 1000) 95% confidence region
4 3.5
0.8 0.6
Probability
NEES
3 2.5
0.4 0.2
2
0 4
1.5
100
2
1
95 90
0
0.5 0
Figure 5. 0.05deg).
5
10 Time (s)
15
20
NEES comparison of spline filter and particle filter (σω =
85 −2
Velocity (m/s)
Figure 7.
80
Position (m)
Predicted pdf of the state.
0.25
0.25
0.2
Probability
Probability
0.2 0.15 0.1
0.15 0.1 0.05
0.05
0 4
0 4
100
2
95
95
2
0
80
−2
Velocity (m/s)
85 −2
Position (m)
Updated pdf of the state.
Prior pdf of the state.
xk = [ηk ξk ] . The system model is given as
1 1 0.5 xk−1 + νk xk = 0 1 1
and xk (43)
= arctan
=
0.5 1 1 −0.4 ν xk−1 + (k − 1) + 0.01 2,k 0 1 0 (46)
and the measurement model is
and the measurement model is zk
80
Position (m)
Figure 8. Figure 6.
85
90
0 Velocity (m/s)
90
yp (k) + ωk ηk − 4k
zk
(44)
where both νk and ωk are zero mean Gaussian random variables with standard deviation 0.05m and 1deg, respectively. The initial target state is [80m 1m/s] and yp (k) = 20m. A spline filter of order 3 is used to track the target, and 10 knots are used for the position dimension and 4 knots for the velocity dimension. Figure 6∼8 show the prior pdf, predicted pdf and updated pdf of the system state at time t = 10s. D. IMM Spline Filter In this subsection, a system with two models is applied to the target. In the second model, a time-varying control term is added. Specifically, the system models are
1 1 0.5 xk = x ν + (45) 0 1 k−1 0.01 1,k
= arctan
yp (k) + ωk ηk − 4k
(47)
where ν1,k , ν2,k and ωk are all zero mean Gaussian random variables with standard deviation 0.8m, 0.8m and 1deg, respectively. The initial target state is [30m 5m/s] and the target starts with model 1 and switches to model 2 at time t = 15s, and yp (k) = 20m. A spline filter of order 3 is used to track the target, and 10 knots are used for the position dimension and 4 knots for the velocity dimension. The estimated model probability is shown in Figure 9, and the target position estimate and velocity estimate are shown in Figure 10 and Figure 11, respectively. From Figure 9∼11, it can be shown that the proposed spline filter is able to track a maneuvering target and detect target model switching. VI. C ONCLUSIONS In this paper, a spline implementation of the Bayesian filter, which provides continuous estimate of the pdf of sys-
1315
1
same as that of the particle filter, thus the spline filter could be used as an effective alternative to the particle filer. In addition, the developed spline filter is free from the degeneracy alike problem due to its continuous nature. Further, the spline filter is able to handle systems with multiple models, which is also justified by the simulation result.
Truth SP
Mode Probability
0.8
0.6
0.4
R EFERENCES
0.2
0 0
5
10
15
20
25
Time (s)
Figure 9.
Mode probability.
100 90
Position (m)
80 70 60 50 40 30 0
Truth SP 5
10
15
20
25
Time (s)
Figure 10.
Position estimate and the truth.
tem state, is developed for multidimensional nonlinear/nonGaussian systems. The analytical solution of the spline filter is developed by representing the system transition model using tensor products of splines. By moving the knots of the spline according to the system transition model, it ensures that the splines always cover the region where the probability of the system state is significant. Simulation results show that the estimation performance of the spline filter is statistically the
10
Truth SP
9
Velocity (m/s)
8 7
[1] Arulampalam, M. S., Maskell, S., Gordon, N., and Clapp, T., “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,” IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 174–188, Feb. 2002. [2] Bar-Shalom, Y., Li, X., and Kirubarajan, T., Estimation with Applications to Tracking and Navigation. John Wiley, New York, 2001. [3] Berzuini, C., Best, N. G., Gilks, W. R., and Larizza, C., “Dynamical conditional independence models and Markov Chain Monte Carlo methods,” Journal of the American Statistical Association, vol. 92, no. 440, pp. 1403–1412, Dec. 1997. [4] Bucy, R. S., and Youssef, H. M., “Nonlinear filter representation via spline functions,” Proceedings of the 5th Symposium on Nonlinear Estimation, pp. 51–60, 1974. [5] De Boor, C., A Practical Guide to Splines. Springer-Verlag, New York, 2007. [6] Daum, F., and Huang, J., “Particle flow for nonlinear filters with loghomotopy,” Proceedings of SPIE, vol. 6969, 2008. [7] Goldenthal, R., and Bercovier, M., “Spline curve approximation and design by optimal control over the knots,” Computing, vol. 72, no. 1, pp. 53–64, Apr. 2004. [8] Gordon, N. J., Salmond, D., and Smith, A., “Novel approach to nonlinear/non-Gaussian Bayesian state estimation,” Radar and Signal Processing, IEE Proceedings F, vol. 140, no. 2, pp. 107–113, Aug. 2002. [9] Julier, S. J., and Uhlmann, J. K., “A new extension of the Kalman filter to nonlinear systems,” Proceedings of AeroSense, Simulation and Controls, vol. 3, 1997. [10] Kocherry, D. L., Tharmarasa, R., Lang, T., and Kirubarajan, T., “Spline filter for target tracking,” Proceedings of SPIE, vol. 7698, 2010. [11] Prautzsch, H., Boehm, W., and Paluszny, M., B´ezier and B-spline techniques. Springer-Verlag, 2002. [12] Punithakumar, K., and Kirubarajan, T., “Spline filter for nonlinear/nonGaussian Bayesian tracking,” Proceedings of SPIE, vol. 6699, 2007. [13] Punithakumar, K., McDonald, M., and Kirubarajan, T., “Spline filter for nonlinear/non-Gaussian Bayesian tracking,” Proceedings of SPIE, vol. 6969, 2008. [14] Septier, F., Pang, S. K., Carmi, A., and Godsill, S., “On MCMCBased particle methods for Bayesian filtering: Application to multitarget tracking,” Computational Advances in Multi-Sensor Adaptive Processing (CAMSAP), 2009 3rd IEEE International Workshop on, Aruba, Dutch Antilles, Feb. 2009. [15] Tharmarasa, R., Kirubarajan, T., Hernandez, M. L., and Sinha, A., “PCRLB-based multisensor array management for multitarget tracking,” IEEE Transactions on Aerospace and Electronic Systems, vol. 43, no. 2, pp. 539–555, Aug. 2007. [16] Van Trees, H., Detection, Estimation and Modulation Theory. vol. I, John Wiley, New York, 1968. [17] Vermeulen, A. H., Bartels, R. H., and Heppler, G. R., “Integrating products of B-splines,” SIAM Journal on Scientific and Statistical Computing, vol. 13, no. 4, pp. 1025–1038, Jul. 1992. [18] Wang, A. H., and Klein, R. L., “Implementation of non-linear estimators using monospline,” Proceedings of the 15th IEEE Conference on Decision and Control, vol. 74, pp. 1305–1307, 1976.
6 5 4 3 2 0
5
10
15
20
25
Time (s)
Figure 11.
Velocity estimate and the truth.
1316
