The Iterated Extended Kalman Particle Filter Li Liang-qun, Ji Hong-bing,Luo Jun-hui School of Electronic Engineering, Xidian University ,Xi’an 710071, China Email: [email protected] Abstract— Particle filtering shows great promise in addressing Simulation results are given in Section 5.Conclusions are a wide variety of non-linear and /or non-Gaussian problem. A presented in Section 6. crucial issue in particle filtering is the selection of the importance proposal distribution. In this paper, the iterated extended 2. PARTICLE FILTER kalman filter (IEKF) is used to generate the proposal distribution. The proposal distribution integrates the latest 2.1 Modeling Assumption observation into system state transition density, so it can match Consider the nonlinear discrete time dynamic system: the posteriori density well. The simulation results show that the new particle filter superiors to the standard particle filter and xkk= fx( k−1,vk−1) (1) the other filters such as the unscented particle filter (UPF), the extended kalman particle filter (PF -EKF), the EKF. zhkk= ( xk,ek) (2) where f :ℜnnxv×ℜ →ℜnxand h :ℜ×nnxeℜ→ℜnz represent the 1. INTRODUCTION k k system evolution function and the measurement model Nonlinear filtering problems arise in many fields n n function. x is the system state at time , z is including statistical signal processing, economics, statistics, xk ∈ℜ k zk ∈ℜ biostatistics, and engineering such as communications, radar n m the measurement vector at time k . vk −1 ∈ Rand eRk ∈ tracking, sonar ranging, target tracking, and satellite represent the process noise and the measurement noise navigation [1-7]. The best-known algorithm to solve the respectively. problem of nonlinear filtering is the extended Kalman filter. 2.2 Particle Filter This filter is based upon the principle of linearizing the The sequential importance-sampling (SIS) algorithm is a measurements and evolution models using Taylor series Monte Carlo method that forms the basis for most sequential expansions. The series approximations in the EKF algorithm MC filters developed over the past decades; see [3]. The can, however, lead to poor representations of the nonlinear sequential MC approach is known variously as bootstrap functions and probability distribution of interest. As a result, filtering, the condensation algorithm, particle filtering etc. this filter can diverge. Another popular solution strategy for The key idea is to represent the required posterior density the general nonlinear filtering problem is to use sequential function by a set of random samples with associated weights Monte Carlo methods, also known as particle filters. and to compute estimates based on these samples and weights. Particle filtering is a class of methods for filtering, Ns smoothing etc. in non-linear and/or non-Gaussian state space Let xwii, denote a random measure that { 0:kk}i=1 models that may perform significantly better than traditional characterizes the posterior pdf px(|z ), methods like the extended Kalman filter. A key issue in 0:kk1: where i is a set of support points with particle filtering is the selection of the proposal distribution {x0:k,0i= ,...,Ns} function. In general, it is hard to design such proposals. Now associated weights wii , = 0,..., N and x ==xj, 0,...,kis there have many proposed distributions have been proposed in { ks} 0:kj{ } the literature. For example, the prior, the EKF Garssian the set of all states up to time k . The weights are normalized approximation and the UKF proposal are used as the proposal such that wi = 1 . Then, the posterior density at k can be distribution for particle filter [2,3,4]. In this paper, we follow ∑i k the same approach, but replace the EKF proposal by an IEKF approximated as proposal. Because the IEKF compute the updated state not as N s p()xz ≈−ωδii(x x) an approximate conditional mean-that is, a linear combination 0:kk1:k ∑ 0:k0:k i=1 (3) of the prediction and the innovation, but as a maximum a posteriori (MAP) estimate [1]. So the IEKF can be used to where pz(|xii)p(x|xi) generate proposal distributions with more precise that are ii kk kk−1 (4) wwkk∝ −1 ii close to the true mean of the target distribution. qx(|kkx−1,zk) This work is organized as follows. The generic particle i is the likelihood function of the measurements filter is formulated in Section 2.The proposed iterated p(|zxkk) extended kalman particle filter is presented in Section 3. ii zk , qx(|kkx−1,zk)is the importance density. It can be state. In the general case, it may not be straightforward to do shown that as Ns →∞, the approximation (1) approaches either of these things. Therefore, the distribution px(|kx0:k−1 ) the true posterior density px(|0:kkz1: ). From the equation (3) and (4), it is show that the particle is the most popular choice of proposal function. Compared filter consists of recursive propagation of the weights and with the optimal proposal distribution, it is attractive due to its support points as each measurement is received sequentially. simplicity in sampling from the prior densities and the Furthermore, a common problem with the particle filter is the calculation of weights. But because it is not incorporating the degeneracy phenomenon. In order to avoid the degeneracy most recent observations, the proposal distribution is very phenomenon of the particles, the resampling scheme is inefficient sometimes, and the estimation result is poor. In important to the particle filter. To the resampling scheme, order to solve this problem, several techniques have been there are many selections such as sampling importance proposed. For example, the EKF or UKF approximation is resampling, residual resamping and minimum variance used as the proposal distribution for a particle filter. In this sampling. In this paper, the residual resampling is used in all section, we will use the IEKF to generate proposal of the experiments. So a generic particle filter is then as distributions with a better performance than the EKF or UKF. described by Algorithm 1. The detail derivation of the 3.1 The Iterated Extended Kalman Filter particle filter can be founded in [3]. The conditional probability density function of xk(+1)given Z k +1 can be written, assuming all the pertinent Algorithm 1: Generic Particle Filter random variables to be gaussian, as 1. Initialization: k = 0 px⎡⎤k+=1|Zkk++11px⎡k+1|z(k+1),Z ⎤ ()i ⎣⎦( ) ⎣( ) ⎦ • For i= 0,..., Ns , draw the states x0 from the 1 (5) =+pz⎡⎤(1k )|x()k+1p⎡⎤x()k+1|Zk c ⎣⎦⎣⎦ prior p()x0 1 2. FOR =+Nz⎡ (1k );h⎡⎤k+1,x()k+1,R()k+1⎤ k = 1,2.... c ⎣ ⎣⎦⎦ ˆ • FOR iN=1: s .(Nx⎣⎦⎡⎤k++1);x()k1|k,P()k+1|k Draw iii Maximizing the above with respect to xk(+1)is equivalent to xkk∼ qx(|xk−1,zk) i minimizing Assign the particle a weight, w , according to (4) k 1 ' −1 J⎡x()k+=11⎤⎡z()kh+−k+1,x(k+1)⎤R()k+1z()kh+1−⎡k+1,x(k+1)⎤ END FOR ⎣ ⎦⎣2{}⎦{}⎣⎦ (6) • FOR iN=1: s 1 ' −1 +⎣⎦⎡⎤xk()+11−+xkˆˆ(|k)P(k+1|k)()⎣⎦⎡⎤xk+1−+xk(1|k) Normalize the weights: 2 ii j The iterative minimization of (6), say, using a Newton- wwkk= wk ∑ jN=1: s Raphson algorithm, will yield an approximate MAP estimate • END FOR of xk(+1). This is done by expanding J in a taylor series • Resample: ()i up to second order about the i th iterated value of the estimate Multiply /Suppress samples x with high/low 0:k i i of xk(+1), denoted (without time argument) as x , importance weights wk , respectively, to obtain ii' i1 i' i i ()i (7) J =+J J()xx− +()xx− Jxx (x−x) Ns random samples x0:k approximately 2 distributed according to where, using abbreviated notation, px(|0:kkz1: ) ' i JJ= | i For iN=1: s , set wNks=1 xx= and JJi =∇ | xxxx= i 3. THE ITERATED EXTENDED KALMAN PARTICLE FILTER JJi =∇ ∇' | xx x x xx= i The choice of proposal function is one of the most are the gradient and Hessian of with respect to . critical design issues in importance sampling algorithm. J xk(1+ ) Doucet, etc [7] proved the optimal proposal distribution Setting the gradient of (7) with respect to x to zero yields the next value of x in the iteration to minimize as q(|xxkk0: −1,z1:k) = px(|kkx0: −1,z1:k)can minimize the variance of ii+1 i−1 i (8) x =−xJ()xxJx the importance weights conditional on x0:k−1 and z1:k . This choice of proposal distribution has also advocated by other The gradient J is, using now the full notation, researchers. However, This optimal importance density suffers two major drawbacks. It requires the ability to sample from px(|kkx0: −1,z1:k)and to evaluate the integral over the new Jhii=− ⎡⎤k+1, xˆ i()k+1 | k+1 '1R(k+1)− Algorithm 2: The Iterated Extended Kalman Particle Filter xx⎣⎦ 1. Initialization: k = 0 (9) .1zk+−h⎡⎤k+1,xˆ i k+1|k+1 ()i {()⎣⎦( )}• For i= 0,..., Ns , draw the states x0 from the − 1 i prior px() ++P (1kk|)⎣⎡xˆˆ()k+1|k+1−x(kk+1|)⎦⎤ 0 2. FOR The Hessian of J , retaining only up to the first k = 1, 2.... • FOR iN=1: derivative of h , is s ii −1 −1 --Compute the Jacobians F& G of the process Jhii=⎡⎤k+1, xˆˆik++1 | k1 Rk+1 h⎡⎤k+1, xik++1 | k1 kk xx x ⎣⎦()()x ⎣⎦() model (10) ++Pk(1|k)−1 --Update the particles with the IEKF: xˆˆii= fx() ii'1− −1 kk+1/ k/k =+Hk(11)R()k+H()k+1+P(k+1|k) ii' i Pk(1+ |k)=+FPk(|k)F G where kkk FOR j =1:c (c is the number of iteration) ii Hk( ++11) hkx ⎡,xˆ (k+1|k+1)⎤ Compute the Jacobians Hi&Ui of the ⎣⎦ kkj j is the Jacobian of the relinearized measurement equation.