A Fast Numerical Fitting Approach to Calculate the Likelihood of Particles in Particle Filters

Transcription

1 1 A Fas Numerical Fiing Approach o Calculae he Likelihood of Paricles in Paricle Filers Tiancheng Li, Shudong Sun and Tariq P. Saar T. Li is wih he School of Mecharonics, Norhwesern Polyechnical Universiy, Xi an, , China. He is also wih he Cener for Auomaed and Roboics NDT, London Souh Bank Universiy, London, SE1 0AA, UK (corresponding auhor, li3@lsbu.ac.uk, iancheng.li1985@gmail.com). S. Sun is wih he School of Mecharonics, Norhwesern Pol yechnical Universiy, Xi an, 71007, China ( sdsun@nwpu.edu.cn). T. P. Saar is wih he Cener for Auomaed and Roboics NDT, London Souh Bank Universiy, Lo n- don, SE1 0AA, UK ( saarp@lsbu.ac.uk). This paper is a preprin of a paper submied o IEEE Transacion on Cyberneics and is subjec o Insiue of Elecrical and Elecronics Engineers (IEEE) Copyrigh. If acceped, he copy of record will be available a IEEE Xplore.

2 Absrac The likelihood compuaion of a huge number of paricles leads o enormous compuaional demands in a class of applicaions of he paricle filer (PF), such as visual arge racking and robo localizaion. To alleviae his compuaional boleneck, we propose a numerical fiing approach for fas compuaion of he likelihood of paricles. In which, he Likelihood Probabiliy Densiy Funcion (Li- PDF) of he paricles is consruced in real-ime by using a small number of socalled fulcrums. The likelihood of paricles is numerically fied by he Li-PDF insead of direcly compuing i from measuremens. In his way, less compuaion is required which enables real ime filering. More imporanly, he Li-PDF based likelihood compuaion is of sufficien qualiy o guaranee accuracy provided an appropriae fiing funcion and sufficien and properly spaced fulcrums are used. Selecion of fiing funcions and fulcrums is addressed in deail wih examples, respecively. In addiion, o deal wih he racable mulivariae fiing problems, we presen he implici form of our Li-PDF approach. The core idea in his approach is he nonparameric Kernel Densiy Esimaor (KDE) which is flexible and convenien for implemenaion. The validiy of our approach is demonsraed by simulaions and experimens on he univariae benchmark model, arge racking and mobile robo localizaion. Index Terms Paricle filer, numerical fiing, arge racking, robo localizaion.

3 3 I. INTRODUCTION This paper concenraes on improving he compuing efficiency of paricle filers for a variey of nonlinear filering problems. Nonlinear filering recursively esimaes in ime he nonlinear sequence of poserior densiies of he sae given a sequence of measuremens. This can be wrien in he form of he discree dynamic sae space model x f x 1, v 1 (sae dynamic equaion) y h x, e (observaion equaion) (1) where indicaes discree ime, x R nx denoes he sae, y R ny denoes he measuremen, v and e denoe sochasic noise affecing he sysem dynamic equaion f : R nx R nv R nx, and he observaion equaion h : R nx R ne R ny, respecively. Furhermore, le x 0: (x 0, x 1,..., x ) and y 0: (y 0, y 1,..., y ) be he hisory pah of he signal and of he observaion process. A convenien soluion o he filering problem is Recursive Bayesian esimaion, which is based on wo assumpions Assumpion 1 The saes follow a firs-order Markov process 0: 1 px x 1 p x x () Assumpion The measuremens are independen of he given saes 0: 1 p y0: 1 p x y p y0: 1 x so long as p( x ) 0 (3) p x Using Bayes' rule, we have he required marginal poserior densiy p x y 0: px y 0: 1 p y y0: 1 p y x (4) This is deermined by wo seps (1) Predicion

4 4 p x y p x y p x x dx (5) 0: 1 1 0: n x () Updaing or correcion p x y 0: n x 0: 1 p y x 0: 1 p x y p x y p y x dx (6) Recursive calculaion or approximaion of hese wo seps is he essence of Bayesian filering. The sae dynamics are a self-driven process ha evolves wih ime. The praciioner merely needs o simulae hem in his compuaion. In various pracical applicaions, he updaing in (6) is hardware-sensiive as well as much more compuaionally expensive han he predicion (5), especially when a high resoluion updaing is execued. This is paricularly rue for visual racking [1,, 3], robo localizaion [1, 4], ec. In hese applicaions, he esimaion accuracy and filering speed are boh resriced heavily by he updaing sep, see also [39]. This fac forms he saring poin of his paper. The enormous compuaional demand will be muliplied when he updaing is required o repea many imes in paricle filers. Paricle filers implemen Bayesian esimaion via Sequenial Mone Carlo (SMC) simulaion [5, 6, 7]. The poserior densiy is represened by a se of random samples (called paricles) wih associaed weighs, and hence he Bayesian ieraion needs o repea N imes when here are N paricles. This sample-approximaion is he srengh bu is also he compuaional boleneck of paricle filers as compared o closed-form soluions like Kalman filers. To reduce he compuaional cos wihou undermining he sample-approximaion abiliy of he PF, he choice lef seems o be o speed up he compuaionally expensive updaing in a way ha does no reduce he number of paricles. For his reason, we propose a numerical fiing approach o calculae he likelihood of paricles insead of direcly compuing i based on measuremens. Numerical fiing [8] has proved o be a very powerful and general mehod for daa predicing and has been used in a range of saisical applicaions where adequae an analyic soluion may no exis. To our knowledge, his is he firs aemp o inroduce he powerful numerical fiing echnique o enhance paricle filers. Our

5 5 approach is based on he undersanding ha he direc likelihood calculaion based on measuremens is compuaionally more expensive as compared wih addiional numerical fiing. The remainder of his paper is organized as follows. A brief sae of he ar in he developmen of paricle filer for fas processing is described in secion II. The concepual framework and implemenaion deails of our approach are given in secion III. Simulaions and experimens are presened in secion IV before we conclude in secion V. II. THE STATE OF THE ART OF FAST PARTICLE FILTERING A long-sanding objecive in he field of paricle filers is o improve heir real-ime performance. One of he mos effecive soluions (o decrease he compuaional burden) is o reduce he number of paricles, see he survey of adapaion mechanisms o reduce he number of paricles [13, 43]. However, cauion has o be exercised on reducing he number of paricles since a small number of paricles makes i hard o approximae he Probabiliy Disribuion Funcion (PDF) properly and o cope wih he resuling informaion imprecision [14]. Schemes o work well wih fewer samples have been proposed such as in [9, 15], bu i is seldom possible o ge a win-win siuaion. Effors have been made o increase filering efficiency by simplifying he updaing sep ha creaes he main compuaional burden bu wihou reducing sample-approximaion abiliy. This is feasible in wo ways. One way is o reduce he number of required updaing cycles. The oher way is o reduce he compuaion of each updaing cycle. In he firs mehod, all he paricles lying in he same pariion of he sae space (i.e. 'grid') are sae-similar and can approximaely share he same updaing likelihood [16] o avoid redundan compuing. The suppor vecor daa descripion densiy esimae [39] provides a sparse represenaion of he disribuion o avoid compuing he weighs of insignifican paricles, hereby reducing he compuaional burden. Similarly, only he measuremens ha lie in he so-called sigma-gaes around he paricle have significan impac o he likelihood and hose ouside of he gae are no aken ino accoun o save compuaion [31]. Anoher criical idea

6 6 proposed in [17] is o use differen number of paricles for predicion and updaing by using fewer paricles for updaing han for predicion. These researches concenrae on reducing he number of compuaionally expensive cycles of updaing while mainaining he necessary number of paricles for propagaion. In he second mehod, he likelihood compuaion [18] depends heavily on he observaion model such as sensors, weighing funcion, ec. Generally, higher resoluion measuremen has a higher compuaional cos. An auo-adjusable measuremen model is proposed in [19] ha can dynamically change beween conneced componen analysis and k-means based model o obain a balance beween racking precision and reduced runime. In [0], synchronous and asynchronous sensor measuremens as well as special cases like sensor failure, and sensor funcioning condiions are considered. A swiching sensor saes model beween hem is presened. To save online calculaion, in [] he reference measuremen informaion of paricles is pre-sored (a he price of exra approximaion errors). Real-ime echniques such as parallel/disribued processing, muli-resoluion/muli-rae processing, dimension reducing, ec. provide possibiliies for processing signals sampled a high raes. However, one of he bigges challenges o developing parallel paricle filers is he resampling operaion ha requires he join processing of all paricles and, herefore, prevens parallelizaion of PFs. To comba his, various soluions for parallel resampling have been proposed, see [1, 9, 33] and he references herein. The core idea of disribued paricle filers is o disribue he paricle filer algorihm among differen compuing agens for fas parallel compuing [3]. Similarly, a muli-rae processing srucure o deal wih sample se updaes a differen raes [4] and furher decomposing he weighs [5] are boh aimed a compuaion saving. Furher, Rao Blackwellizaion (RB) [6, 4], Subspace hierarchical approach [7, 35], and Mode Tracking (MT) by spliing he sae space [8] are all effecive mehods o reduce he dimensions of he sae space ha needs o be processed. The idea of RB is o divide he sae so ha he Kalman filer is used for he par of he sae ha is linear

7 7 and he paricle filer is used for he oher par. For example in [3] human posiion is racked by a Kalman filer whereas human body pars are racked using a se of paricle filers. In order o remove he linear dependence of using he Kalman filer, he Decenralized Paricle Filer (DPF) [40] splis he filering problem ino wo nesed sub-problems and hen handles he wo nesed subproblems using PFs. This differs from RB because he disribuion of he second group of variables is also approximaed by a condiional paricle filer. Furher, muliple paricle filer [41] may pariion he sae space ino more subspace and run separae paricle filers in each subspace. A similar idea is implemened in [6] which represens each componen as a single-chain Bayesian nework and use paricle filering o rack each componen for muli-componen racking. Furher, ime-scale separaion is exhibied in [10] ha allows wo simplificaions of he paricle filer: 1) use he averaging principle for he dimensional reducion of he dynamics for each paricle during he predicion sep and ) facorize he ransiion probabiliy for he RB of he updae sep. The resuling paricle filer is faser and has smaller variance han he paricle filer based on he original sysem. The exension of paricle filers should consider heir pracical applicaions. Comparing wih he mulicore processors and FPGAs (field programmable gae arrays) [9], GPUs (graphics processing unis) offer low cos and easily accessible single insrucion muliple daa (SIMD) parallel hardware ha is suiable for fas and parallel processing. Therefore, here is an increasing ineres in uilizing GPUs o accelerae he parallel compuaion of PFs since he pioneering aemps, see [1]. A ypical assumpion underlying paricle filers is ha all samples can be updaed whenever new sensor informaion arrives. Under real-ime condiions, however, i is possible ha he updae canno be compleed before he nex sensor measuremen arrives. This can be he case for compuaionally complex sensor models or whenever he underlying poserior requires large sample ses, see [30]. To avoid he loss of measuremens when he rae of incoming sensor daa is higher han he updaing rae, a mixure of individual sample ses are used in he real-ime paricle filer [30]. This is done by considering

8 8 all sensor measuremens and by disribuing he samples among he measuremens wihin an updae window (one esimaion inerval). In our view, he bes way o avoid measuremen loss is o improve he processing speed of paricle filers in pracical applicaions. III. LI-PDF BASED WEIGHT UPDATING A. The concepual framework For nonlinear filering, here are mainly wo kinds of approximaions feasible when no analyic opimal soluion exiss: analyic approximaions and numerical approximaions. The analyic approximaions linearize he nonlinear funcions in he sae dynamic and measuremen models and apply hem direcly o he convenional linear recursive algorihm, such as he exended Kalman filer. The numerical mehods approximae numerical inegraion in nonlinear filering using numerical mehods such as sample approximaion e.g. he Poin-Mass (PM) filer [11, 1], he unscened filer and he paricle filer, ec. Our approach applies an analyic approximaion (numerical fiing) echnique in sample approximaion (Paricle filer) for nonlinear filering. The essence of paricle filers is o evaluae inegrals namely he poserior PDF by a se of paricles x (i) wih associaed non-negaive weigh w (i) ha employs he srong law of large numbers (SLLN), i.e. N ( i) ( i) 0: 1 1, 0: 1 p x y p y x w p x x y (7) i1 where N is he number of paricles, =0 denoes he iniialized paricles se. The weigh of paricles is deermined based on Sequenial Imporance Sampling (SIS) ( i) ( i) ( i) 1 i i qx x0: 1, y1: p y x p x x w w, w 1 (8) N ( i) ( i) ( i) 1 ( ) ( ) i1 where q( ) is he proposal imporan densiy, also called evidence. For simpliciy, he proposal may be chosen as he sysemaic dynamic p(x x -1 ). Moreover, resampling may be applied o rese pari-

9 9 cles' weigh, namely he basic Sequenial Imporance Resampling (SIR) paricle filer. This paper focuses on using numerical fiing of he Probabiliy Densiy Funcion of he Likelihood (Li-PDF) o calculae he likelihood of paricles insead of using direc measuremens ( i ) ( i) p y x L x (9) where L ( ) is he Li-PDF a ime, which can be eiher explici or implici as defined laer. The following wo definiions lie a he core of our approach. Definiion 1. The ask of numerical fiing is o recover y=f(x; C) from a given daa based on he belief ha his daa conains a slowly varying componen, which capures he rend of, or he informaion abou, y, and a varying componen of comparaively small ampliude which is he error or noise in he daa. There are wo forms of numerical fiing: regression and inerpolaion, disinguished from each oher on he basis of wheher he funcion works on he daa (inerpolaion) or no (regression). Definiion. The given daa poins used for numerical fiing in our approach are called fulcrums. Fulcrums and paricles have he same characerisics, see secion A. In simple words, wih he Li-PDF approach he likelihood of he paricles is obained by numerical fiing he measuremen likelihoods of he fulcrums. This is depiced simply as in Figure 1. In which, he horizonal axis and he verical axis represen he sae and likelihood respecively. The boxes represen he fulcrums and heir likelihoods namely he heigh of he red doed lines which are already known, namely p(y x). The objecive of our approach is o calculae he required likelihood of paricles which are represened by circles. To achieve his, he likelihoods of he fulcrums are fied wih heir sae o ge he Li-PDF namely he red curve and hen his curve is used o fi/obain he paricles' likelihood, namely he heigh of black lines. In his way, no maer how many paricles here are, we can fi all of hem o ge heir likelihoods by using he Li-PDF curve insead of direc measuremen-based calculaion of p(y x).

10 Likelihood value Paricles Fulcrums Likelihoods of fulcrums Li-PDF curve Fied likelihood of paricles Sae Fig.1 Schemaic diagram of he Li-PDF approach for likelihood calculaion The framework of he explici Li-PDF based paricle filer is described as in Algorihm 1 wih deails and explanaion of erminology given in he following subsecions. Algorihm 1: Explici Li-PDF based Paricle Filering Ieraion Inpu: S -1 = x Oupu: S = ( i ) ( i ) N 1, w 1 i1 ( i ) ( i, ) N x w i 1 1. Selecive Resampling (do if he variance of he non-normalized weighs is greaer han a prespecified hreshold) Resample N un-weighed measure x. Predicion For i=1 N, sample from he proposal 3. Li-PDF consrucion () i 1,1/ N 3.1 Consruc M fulcrums: (X 1, X,, X M ) N i 1 from S -1 () i ( i) ( i) x ~ qx, x 1 y 3. Calculae heir likelihoods: Y m =p(y x ) of X m

11 Numerically fi he likelihood of fulcrums wih heir saes o ge he Li-PDF L ( ), which saisfies: (Y 1, Y,, Y m ) L (X 1, X,, X m ) 4. Updaing Updae he weighs of paricles using heir fied value of L ( ) For i=1 N, w ( i) ( i) ( i) 1 0: 1, 1: L x p x x ( i) ( i) w 1 ( i) ( i) q x x y Normalize he weighs: ( i) ( i) N ( j) / j w w w Remark 1. Approximaing he densiy of ineres, e.g. he poserior densiy, by a coninuous analyical funcion is an inuiive idea for precise and easy-o-handle approximaion. Previous works have proved he efficiency of his idea. The regularized paricle filer (RPF) [47] and kernel paricle filer (KPF) [44] use he kernel densiy esimaor (KDE) o approximae he poserior PDF. Also based on kernels, [45, 46] use Gaussian mixures (GMs) o represen he poserior PDF as well as he measuremen likelihood [45, 46] funcion. As heir core idea, he coninuous funcion in he form of eiher kernels [44, 47] or GMs [45, 46] is propagaed over ime and is used o calculae he filer esimae. In our Li-PDF approach boh he ideas of inferring he likelihood of paricles by ha of ohers and using he numerical fiing echnique o consruc arbirary likelihood PDF funcion are applied for he firs ime. B. Non-negligible suppor fulcrums There are wo mehods o consruc fulcrums: One mehod is jus o selec some paricles from he paricle se (non-uniformly disribued) and he oher is o creae new daa-poins in he sae space (uniformly disribued). More fulcrums are more likely o ge a beer fiing approximaion bu a he

12 1 cos of more compuaion. The fulcrums should be disribued appropriaely so ha hey have an adequae represenaion of all he paricles wih he fewes possible fulcrums. For efficien numerical fiing, a grid-based mehod is adoped o generae uniformly disribued fulcrums ha cover he non-negligible region. By pariioning he sae space of paricles ino recangular cells, he fulcrums are he ceners of hose cells (grid poins). This mehod o approximae probabiliy densiy by recangular delimied daa-poins is also implemened in he PM filer. As shown laer, he anicipaive boundary-based grid design and he non-negligible suppor principle proposed in [11, 1] are also suiable for our approach by a sensible conversion from he predicive PDF in PM filer o he Li-PDF. In conras o inerpolaion ha predics wihin he range of values in he daase used for model fiing, predicion ouside his range of he daa is known as exrapolaion. The furher he exrapolaion goes ouside he daa, he more likely i is for he model o fail due o differences beween he fiing assumpions and he sample daa or he rue values. To avoid his, fulcrums are consruced in he sae space I o cover he complee sae-space of paricles wih a boundary margin r [ ] [ ] [ ] [ ] [ ] I min x r,max x r 1, L (10) where x [l] is he sae value in he lh coordinae, L is he oal dimensionaliy, r [l] is he lh dimension boundary margin which will be deermined on he measuremen noise as in [11] [ ] [ ] r a Q (11) where Q is he measuremen noise variance marices and a is he design parameer ha deermines he non-negligible suppor of he measuremen noise. I remains o be shown how many fulcrums are required. One adapive echnique for seing he number of grid poins [11] is ha he number of daa poins M [l] saisfies M I [ ] [ ] [ ] 1/ Q x h x max de x (1)

13 13 where Ω is a significan suppor of he predicive PDF p(x y -1 ), h(x) is he measuremen funcion, γ>0 is he second design parameer. As h(x) is acually unknown and supposed o be approximaed by he fiing funcion L (x), subsiue i by L (x) o ge M I L x [ ] [ ] [ ] 1/ Q max de x x (13) and he oal number of fulcrums is M L [ ] M 1 (14) If we delimi he fulcrums wih a fixed inerval d [l], namely d [ ] M [ ] I [ ] 1 (15) Then, he fulcrums can be defined a he space crossing of he following coordinaes x [ ] [ ] [ ] [ ] [ ], m min x r m 1 d m 1, M (16) There is no doub ha he larger he measuremen noise is, he more fulcrums i requires. Choosing a sensible number of fulcrums wih respec o he measuremen noise is imporan in our approach. For simpliciy and fas online compuaion in muliple dimension siuaions, he following number M [l] is suggesed in our applicaion such ha [ ] M p or 1 1, L (17) where p is a specified value loosely saisfying (13), M [l] =1 means he insignifican lh dimensionaliy is no pariioned. To noe, fulcrums could be added ino he paricle se as normal paricles o expand is esimaionspace. This will no increase addiional likelihood compuaion as he likelihood of he fulcrums has already been calculaed. Obviously, he oal number of paricles may be increased ha will affec he compuaion of oher pars of he filer unless a soluion is found o remove some normal paricles.

14 14 C. Leas squares numerical fiing Numerical fiing is accomplished in pracice by picking a linear or nonlinear funcion y f x c c c ; 1,..., k ha depends on cerain parameers c 1, c, c k. To noe, he fiing daa may no sricly work on he funcion bu insead a fiing error generally exiss, i.e. mahemaically m m 1 k m y f x ; c, c..., c e (18) where y m is measured value of he dependen variable, c 1, c, c k are he required parameers. In our approach, he given daa (x m, y m ), m=1,, M are fulcrums, x is he sae, y is he likelihood, and f( ) is he required Li-PDF. The dependence of he likelihood funcion on he parameers can be eiher linear or nonlinear. For he nonlinear likelihood funcion, soluions include approximae linearizaion wih olerable errors (given in appendix A) and conversion mehods of he nonlineariy (see subsecion E). Oherwise, some nonlinear regression mehod is required, such as he Gaussian-Newon mehod. The Gaussian- Newon mehod is one algorihm for minimizing he sum of he squares of he residuals beween daa and nonlinear equaions, in which he leas-squares heory may be used. In he following, we firs consider basic linear univariae variable fiing while he inracable mulivariae fiing wih a local smoohing sraegy will be described in subsecion D. Normally, one will ry o selec a funcion L(x) ha depends linearly on he parameers, in he form L( x) c ( x) c ( x)... c ( x) (19) 1 1 k k where {Φ i (x)}are a-priori seleced se of funcions, for example, he se of monomials {x i-1 } or he se of rigonomeric funcions {sinπix}, and he {c i } are parameers which mus be deermined. In he following, we call k he order of he fiing funcion. In over-deermined sysems, as in our case, k is much smaller han he number M of fulcrums M k (0)

15 15 To specify he form of he funcions in (19), he bes case is when he funcion is known in advance. Oherwise, reasonable assumpions and off-line searching for he opimal fiing model is necessary. To find he opimal fiing model, he Goodness-of-fi may be applied (see laer and appendix B). Once he approximaing funcion form and fulcrums have been defined, as explained laer in secions B and C respecively, he nex sep is o deermine he populaion parameers c 1, c,, c k o ge a good approximaion. As a general idea, he residuals m m m 1 k d f L x ; c, c..., c m 1,,... M (1) are simulaneously made as small as possible. One ries o make some norm of he M-vecor d=[d 1, d,, d M ] T as small as possible - ypically such as he -norm d M 1/ dm () m1 This leads o a linear sysem of equaions for he deerminaion of he minimum ĉ k s. The resuling approximaion L(x; ĉ 1, ĉ,, ĉ k ) is known as he leas-squares approximaion o he given daa and ĉ k s are called leas squares esimaes of he populaion parameers [3]. An appropriae fiing model and well-disribued and sufficien fulcrums are wo criical facors o achieve good fiing resuls. The Goodness-of-fi could be esed o decide if we may proceed or wheher we need o search for a more suiable Li-PDF model, one ha will beer represen he rue observaion measuremen. Available Goodness-of-fi (Gof) ess include he Kolmogorov-Smirnov es, Anderson-Darling es, Chi-Square es, ec. Appendix B gives he Chi-Square es o measure he compaibiliy of paricle likelihoods wih heir fied values of Li-PDF based on he empirical disribuion funcion. This affords a principle o find he opimal fiing model. Under he assumpion of classical linear regression model and normaliy of he residuals erm, Leas-square based numerical fiing can ge he bes Chi-Square Goodness-of-fi es resul [3] since Leas-squares in our approach agrees wih he Chi-Square es of fulcrums in he following form

16 16 min O E min red (3) D. Piecewise fiing funcion and Kernel densiy esimaion For many pracical sysems, however, i is difficul or even impossible o find a single funcion o represen he likelihood funcion in he enire sae space, especially for he inracable mulivariae fiing (Hyper-surface problem). As such, a flexible piecewise consan form (lower order funcion) could be chosen. Accompanied wih he piecewise fiing sraegy namely local regression, he linearizaion of he nonlinear dependence on parameers will be more heoreically enable and easier o implemen. This can also reduce he required fiing funcion order ha promises smaller linearizaion error (see also appendix A for explanaion). To do Piecewise/Segmened fiing, he independen variable is pariioned ino inervals and a separae segmen is fied o each inerval and he boundaries beween he segmens. Then he fiing funcion is a sequence of grafed sub funcions ; ; ; L x C F x C x x x F x C x x x 3 (4) F x; C x x x r r r r 1 where x 1, x,, x r are called join poins which are boundaries beween inervals. In our curren approach, sub funcions F i (x; C i ) are of he same order k, he number of fulcrums M i in each inerval (including wo join poins) saisfies M k 1 i 1, r (5) i I is shown in [34] ha he piecewise consan approximaions are bes when he densiies are reasonably smooh in he scale of he grid. This indicaes ha he piecewise inervals should be pariioned in a way ha he likelihood PDF in each inerval is reasonably lower-order smooh. Remark. The smoohness propery of numerical fiing will slow down he weigh concenraing

17 17 of paricles as i reduces he high likelihood bu increases he low likelihood. This will be helpful o alleviae eiher he sample degeneracy or impoverishmen in paricle filers. Noe ha our goal is o calculae he likelihood of paricles bu no o obain he Li-PDF which is only an inermediae process. Thus, in he piecewise fiing, we can use nonparameric local smoohing echniques, e.g. Kernel Densiy Esimaor (KDE), o derive he likelihood of paricles by usinghe fulcrums wihou explicily obaining he Li-PDF. This mehod, ermed as implici Li-PDF, will grealy simplify he mulivariae fiing and is very convenien for implemenaion. In he following, we will illusrae he process. For a paricle x (j), denoe is neares M j fulcrums in a limied scale and heir likelihoods as {x i, p i } i=1,,.., Mj, he required likelihood p(y (j) x (j) ) KDE can be defined as he Nadaraya-Wason kernel-weighed average of hese fulcrum likelihoods p y x M j ( j) ( j) i1 M j i1 K x, x p ( j) h i i K x, x ( j) h i (6) where he kernel ( j) x ( ) x j i Kh x, xi D h ( j) x (7) hλ is a specified bandwidh ermed he inerval widh and D() is a posiive real valued funcion, whose value does no increase wih increasing disance beween x (j) and x i. The local smooher derives he likelihoods of paricles direcly from fulcrums. There are wo quie convenien kernel smoohers available which are widely used in engineering applicaions: neares neighbor smooher and uniform kernel average smooher. The idea of he neares neighbor smooher is as follows. For each poin x (j), ake M j neares neighbor fulcrums x i and esimae he likelihood of he paricle p(y (j) x (j) ) by averaging he values of hese neighbors likelihood. Formally, for (6) ( j) h, i K x x h (8)

18 18 In conras o his, he uniform kernel funcion can be defined as K x, x ( j) h i ( j) x h x i (9) As he esimae of his uniform kernel smooher, every fulcrum x i in he bounded inerval h conribues o he likelihood of he paricle x (j) in inversely proporional o heir disance o he paricle. The convenience of neares neighbor smooher and uniform kernel average smooher will be verified and used in our simulaions laer. Remark 3. The kernel funcion (possibly) used in he implici Li-PDF approach is merely for inerpolaion of he likelihood of paricles and hey will neiher be propagaed over ime nor approximae he poserior PDF as is done in [44-47]. The goal of [44-47] is o obain an opimal approximaion of he poserior PDF while he goal of our Li-PDF approach is o obain fas compuaion by reducing he measuremen-based likelihood calculaion. E. A polynomial fiing example Aiming o boos he real-ime performance of paricle filers hrough reducing direc likelihood compuaion and hereby he ime cos, i is very imporan for our approach o guaranee he approximaion accuracy. To have an inuiive view of he numerical fiing process of Li-PDF and is resuls, he following univariae nonsaionary growh model is considered which is popular in he communiy [5, 6, 7]. The sysem dynamic and measuremen equaions are respecively x 5x x 8cos 1.( 1) e x 1 (30) y 0.05x v (31) where e and v are zero mean Gaussian random variables wih variance 10 and 1 respecively. Assuming he unknown measuremen equaion is y (i) =g(x (i) ), he likelihood funcion can be obained hrough one more sep, i.e. he following Gaussian model

19 19 L () i y y () i 1 x exp (3) (i) where y is he real measuremens and y is he measuremen of paricle/fulcrum x (i). As shown, he likelihood funcion is in fac nonlinear. Insead of using nonlinear fiing mehods ha can be complex, one may linearize he nonlineariy. Equaion (3) can be linearized by aking is naural logarihm o yield ln L x () i 1 ln () i y y (33) Thus, he funcion lnl (x) wih independen variable x has a linear dependence on he parameers. Then, we only need o fi he measuremen funcion y (i) =g(x (i) ). For his, fulcrums can be uniformly disribued wih parameer r=1 in (11), and he -order polynomial in he following rinomial form is assumed as he measuremen equaion y c x c x c (34) 3 1 Then we ge he following Li-PDF L ( i) ( i) y c3x cx c1 () i 1 x exp (35) This is he nonlinear conversion [8] ha changes he nonlinear fiing funcion o a linear one. In Figure, he perfec rue measuremens (y=0.05x ) wihou noise are shown in black and is direc noisy measuremens in (3) of 100 random samples are shown in red circles. The leas squares fiing resuls of he noisy measuremens in (34) of M fulcrums (M=5, 10, 30, 50 separaely) are shown wih colored lines. The fied funcions are as follows:

20 y 0 y x x (5 fulcrums) y x x (10 fulcrums) y x x (30 fulcrums) y x x (50 fulcrums) The resuls show ha our numerical fiing approach ges more accurae measuremens han direc observaion. These good resuls benefi from our pre-knowledge ha he measuremen equaion is a -order polynomial, alhough his is a fairly weak assumpion. Indeed, he more we know abou he measuremen model, he beer he fiing resuls. For example, if we know he fiing funcion is in he monomial form y c3x (36) we ge more precise fiing resuls using he same fulcrums as above (in one rial): c 3 = (5 fulcrums), c 3 =0.0518(10 fulcrums), c 3 = (30 fulcrums), c 3 = (50 fulcrums). The resuls are shown in Figure Perfec measuremens wihou noise Direc measuremens by Eq.(31) Numerical fiing of 5 noisy measuremens Numerical fiing of 10 noisy measuremens Numerical fiing of 30 noisy measuremens Numerical fiing of 50 noisy measuremens x Fig. Measuremens wihou noise, measuremens wih noise and Eq. (34)-based fiing funcion using differen number of fulcrums

21 y Perfec measuremens wihou noise Direc measuremens by Eq.(31) Numerical fiing of 5 noisy measuremens Numerical fiing of 10 noisy measuremens Numerical fiing of 30 noisy measuremens Numerical fiing of 50 noisy measuremens x Fig. 3 Measuremens wihou noise, measuremens wih noise and Eq. (36)-based fiing funcion using differen number of fulcrums IV. SIMULATIONS AND EXPERIMENTS In order o verify he validiy of our approach, hree ypical nonlinear filering problems are considered in he simulaions. They are he aforemenioned univariae nonsaionary growh model, visual arge racking, and mobile robo localizaion. The PF uses he weighed mean of paricle sae is as he esimae and he sysemaic resampling mehod is used. A. Univariae nonsaionary growh model For he nonlinear sysem described in equaions (30), (31), he roo mean square error (RMSE) is used o evaluae he esimaion accuracy, which is calculaed by 1 RMSE T 1/ x ˆ x (37) T 1 where ˆx is he esimae of he sae, T is he sum of ieraions. A big T=10,000 is chosen and a sequence of he number of paricles from 10 o 500 wih inerval of 10 is separaely used.

22 RMSE Firsly, differen orders of polynomial and 10 fulcrums are used in he regression model (34) o fi he measuremen funcion. The RMSE resuls of he Li-PDF based paricle filers are given in Figure 4, from which we can see ha he 1s-order polynomial fiing resul is really poor whereas nd-order and higher form polynomials ge much beer esimaion accuracy. This indicaes ha a proper (no smaller han he rue order) fiing funcion is criically imporan for our approach. Secondly, he RMSE and compuing-ime comparison resuls of he basic paricle filer and he Li- PDF based paricle filers using differen numbers of fulcrums (for nd-order fiing polynomial) are given respecively in Figure 5 and Figure 6. The resuls indicae ha he Li-PDF based paricle filer can obain he same esimaion accuracy bu a he price of more compuaional cos. The reason is ha he compuing speed can be only improved when he ime consumpion for he fiing is less han he likelihood compuaion i has saved. Since he updaing sep (31) is nohing else bu jus he job of solving (34), i is no surprising ha he compuing speed of our approach has no been improved bu insead reduced in his simulaion. As noed a firs, his is no he case of many pracical problems like visual arge racking [6, 35] and robo localizaion [4]. This will be illusraed in he following secions B and C. In boh of which, he measuremen updaing is much more complicaed and ime-consuming han he sae predicion Basic PF Li-PDF based PF (1 order polynomial) Li-PDF based PF ( order polynomial) Li-PDF based PF (3 order polynomial) Number of paricles Fig.4 RMSE of he basic SIR PF and he Li-PDF based PFs ha use differen order of polynomial

23 Time (s) RMSE Basic PF Li-PDF based PF (5 fulcrums) Li-PDF based PF (10 fulcrums) Li-PDF based PF (30 fulcrums) Li-PDF based PF (50 fulcrums) Number of paricles Fig.5 RMSE of he basic PF and he Li-PDF based PFs ha use differen number of fulcrums Basic PF Li-PDF-based PF (5 fulcrums) Li-PDF based PF (10 fulcrums) Li-PDF based PF (30 fulcrums) Li-PDF based PF (50 fulcrums) Number of paricles Fig.6 Processing ime of he basic PF and he Li-PDF based PFs using differen number of fulcrums

24 4 B. Color hisograms based arge racking In his insance, we apply paricle filers o rack a helicoper in a video. To enhance he reproducibiliy of he experimen, we adop a public insance shared by Sébasien Paris based on he color hisogram measuremen model proposed by [36] which is available on hp:// According o he color hisograms based observaion model in [36], he color disribuion p y ={p (u) y } u=1,,,m a locaion y is calculaed as I ( u) y x py f k h x u i1 a i i (38) where I is he number of pixels in he region (I=10 in our case), δ( ) is he Kronecker dela funcion, k( ) is a weighing funcion of pixels, he parameer a is used o adap he size of he region, and f is m ( u) he normalizaion facor. I is saisfied ha p 1 u1 y. k r 1 r 0 r 1 oherwise (39) a H H (40) x y f I i1 1 y xi k a (41) where H x, H y are he lengh of he half x, y separae axes of he ellipse used o deermine he color disribuion. We iniialize he sae space for he firs frame manually. Each sample of he disribuion represens an ellipse and is given as s x, x, y, y, H, H, a (4) x y

25 5 where x, y specify he locaion of he ellipse, x, y he moion, and a is he corresponding scale change. The Li-PDF numerical fiing is currenly implemened in he -dimesion posiion space x x, y (43) The sysem dynamics are described by a firs-order auo-regressive model given as: s 1 As N 0, R (44) where N sands for Gaussian disribuion, Marix A defines he deerminisic componen of he model and R is he covariance as Rx , A R 0 Re where =0.7 in our case as he racking video is pariioned ino 400 frames, R x and R e are he posiion covariance and he ellipse covariance respecively. R 3 / 3 / H x / 0 0 x, 0 0 x R 3 e H y 0 0 / 3 / 0 0 H 0 0 / where, δ x =0.35, δ Hx =0.1, δ Hy =0.1, δ Hθ =π/60. For he observaion model, he likelihood of paricles are proporional o he Bhaacharyya disance beween he color window of he prediced locaion in he nh frame p n ={p (u) } u=1,,,m and he reference q={q (u) } u=1,,,m d 1 [ p, q] 1 1 p( sn) e e (45) where σ is he measuremen noise σ = 0., d is he Bhaacharyya disance defined as

26 6 d 1 p, q (46) ρ[p, q] is he Bhaacharyya coefficien. In our discree case, he hisograms are calculaed in he HSV space using discree bins color window (m=56 ) as follows m ( u) ( u) p, q p q (47) u1 I can be seen ha he measuremen updaing funcion is much more compuaionally expensive han he sae dynamic funcion. In he conras experimens, he same number of paricles is used as in he basic PF and he Li- PDF based PFs. For he Li-PDF approaches, i is se r [l] =5 in (11), p=10 in (17) so ha 100 fulcrums are used. The Li-PDF is direcly consruced by using he neares neighbour based griddaa fiing funcion in Malab, which is applicable o he muliple dimension case. To show he resuls, he racking video snapsho of our approach and rajecories comparison wih basic PF when using 500 paricles are given in Figure 7 and Figure 8 respecively. Evaluaing a racking algorihm in he real world is iself a challenge. Currenly i is hard o compare heir esimaion accuracy for we do no have accurae daa of he rue rajecory. Bu, in our 0 rials, he racking is los 5 imes by he basic PF and he same 5 imes by our approach. This may indicae similar esimaion robusness beween our approach and he basic PF. To have an insigh ino he Li-PDF, he Li-PDF surf is ploed in Figure 9 and he discrepancy beween he likelihood of paricles ha are obained by he Li-PDF approach and by direc calculaion is ploed in Figure 10. Noe ha some regions/paricles may be weighed negaive in our approach as shown in Figure 9. This makes sense for numerical fiing bu no for he PF. To correc his, he negaive weighs of paricles can be se o zero. Since here is always noise involved wih measuremens he likelihood calculaion in boh he basic SIR and our approach are biased from he ground ruh and is independenly noisy.

27 7 The real-ime performances of differen filers are given in able I. I can be seen ha he processing ime of he Li-PDF based PF does no linearly increase wih he number of paricles bu he basic PF does. This demonsraes ha he compuaional complexiy of our approach is no longer heavily limied by he number of paricles N (bu insead i depends more on he number M of fulcrums used). This compares favourably wih he compuaional cos of mos curren PFs if a small number of fulcrums are used. This will be appealing for he case ha requires exremely massive number of paricles [38]. As saed, his is because he evolving sysem dynamics is really compuaionally nohing as compared o he likelihood compuaion based on color hisograms. TABLE I REAL-TIME PERFORMANCE OF PARTICLE FILTERS (SECOND) Number of paricles Basic PF Li-PDF PF Fig.7 Snapsho of he las frame (red curve represens he esimaed rajecory of our approach)

28 Likelihood 8 50 Basic SIR PF Li-PDF based SIR PF Fig.8 Helicoper racking rajecories comparison of he basic SIR PF and he Li-PDF based PF Y X Fig.9 Li-PDF surf of fulcrums for he las frame (he yellow poins represen he likelihood of paricles ha are calculaed direcly based on measuremens)

29 Likelihood discrepency 9 x Y X Fig.10 Discrepancy beween he likelihood of paricles ha are inerpolaed by he Li-PDF approach and direcly calculaed based on measuremens C. Mobile Robo localizaion Mobile robo localizaion is he opimal esimaion of he locaion (and orienaion) of a mobile robo. The applicaion of he paricle filer o mobile robo localizaion is usually named as he Mone Carlo localizaion (MCL) mehod [4]. In order o develop he deails of MCL, le x =(x, y, θ) T (he robo s posiion in Caresian space (x, y) along wih is heading direcion θ) denoes he robo s sae a ime insan, y is he measuremen a ime, and u is he odomery daa (conrol measuremen) beween ime -1 and. The predicion and measuremen updaing in MCL is performed from he moion model and he percepual model as in he following 1, 1 1, 1 1 1, p x y u p x x u p x y u (48) p x y, u 1 1, 1 p y y 1, u 1 p y x p x y u (49)

30 30 Supposing u 1 has a movemen effec ( s, θ) T on he robo, s is he ranslaional disance, and θ he change of robo s heading direcion from ime -1 o. Then, he moion model p(x x 1, u 1 ) can be easily obained as cos 1 0 s x x sin 0 v (50) where v -1 is sysem Gaussian whie noise wih zero mean and [ s 0%, θ 5%] T variance in our case. In paricular, he paricle which falls ino obsrucs will be discarded (by seing is weigh o zero) and be replaced by resampling. The likelihood-based weigh updaing p(y x ) depends on he percepual daa which can be proximiy daa (scanning lidar or sonar) or more complex daa from image/video vision. The nearesneighbor daa associaion used for scan maching in our simulaion can be described as 1 1 p y x y y S y y n ( ) S T 1 exp i j i, j i j i, j (51) where S i, j is he covariance marix of he difference ỹ i -y j, he Gaussian measuremen noise is s~n(0, 5) for each scanning disance, n is he number of scanning lines and we choose n=36 and 180 respecively in our case. A bigger n indicaes a higher resoluion and more ime-consuming likelihood calculaion (51). The Li-PDFs could be consruced in he enire sae space (x, y, θ) or for simpliciy in he Caresian space (x, y) only. This simplificaion is possible because he direcion θ relies srongly on is posiion (x, y) if he measuremens and he covariance marix S i, j are known. In his case, i is se r [l] =1 in (11), p=10 in (17) so ha 100 fulcrums are used in he posiion space (x, y) and he linear uniform fiing mehod is adoped. In addiion, our Li-PDF approach is suggesed o apply from =3 since a he global-localizaion sage paricles are very widely disribued (e.g. he case is ploed in Figure 11 for he second sop) which is unsuiable for consrucing he Li-PDF.

31 31 The robo running pah is also shown in figure 11, from 'S' o 'T'. The discree poins represen paricles and he recangle frame represens he robo. The disribuion of paricles, surfaces of Li-PDF and he discrepancy beween likelihood of paricles ha are obained by he Li-PDF and by direc measuremen-based calculaion a wo sops (when 500 paricles and 100 fulcrums are used and i is se n=36) are given in Figure 14. To evaluae he filering performance, he Euclidean Disance (ED) is defined beween he esimae and he rue posiion of he robo (x, y) ha is calculaed by ED x x y y (5) where ( x,ỹ) is he esimaion posiion of he robo. Boh he number N of paricles and he number M of fulcrums are criical o he performance of PFs. To capure he average performance, we run 100 MC rials. The EDs when differen numbers N of paricles are used are ploed by ime seps in Figure 1 which indicaes ha, our Li-PDF approach has indeed reduced he esimaion accuracy somewha as compared wih he basic SIR PF. This is because he simulaion is based on a highly nonlinear model bu our approach adops he piecewise linear fiing mehod. However, he esimaion accuracy is no very bad since for a huge number of paricles e.g. 1000, a small number of fulcrums (100) can fi he likelihood efficienly. The mean ED from sops 3 o 4 agains he number M of fulcrums is ploed in Figure 13, which shows ha he larger he number of fulcrums, he more accurae is he approximaion. The processing ime of he Li-PDF based PF and basic PF agains he number of paricles are given in Table II and Table III respecively for he scanning daa size n=36 and 180. The resuls demonsrae again he fas processing advanage of our Li-PDF approaches (ha is no limied so heavily by he number of paricles), especially when a high-resoluion measuremen (n=180) is applied. In summary, here is always a rade-off beween increasing he processing speed by reducing likelihood calculaion and improving he esimaion accuracy by mainaining sufficien likelihood calcu-

32 3 laion. As such, i is highly recommended o use an off-line search for he opimal number of fulcrums, as well as he fiing model as aforemenioned, according o he pracical applicaion. The choice also depends on he praciioner s preference for esimaion accuracy and processing speed. Our Li-PDF approach provides a choice for applicaions in which a fas processing speed is much preferred. Fig.11 D simulaion environmen [4] and he paricles disribuion (black poin) when he robo is a he second sop (Recangular represen differen sops)

33 Mean ED Esimaion Error (ED) ED Basic SIR PF N=100 Li-PDF based PF N=100 Basic SIR PF N=00 Li-PDF based PF N=00 Basic SIR PF N=500 Li-PDF based PF N=500 Basic SIR PF N=1000 Li-PDF based PF N= Sep Fig.1 Esimaion error by seps when differen number of paricles are used (n=36, M=100 in Li- PDF PFs) 6 5 N=100 N=00 N=500 N= The number of fulcrums Fig.13 Mean esimaion error agains he number of fulcrums (n=36)

34 Likelihood discrepency Likelihood discrepency Likelihood Likelihood 34 x 10-3 x Y X Y X Y X Y X Fig.14 Disribuion of paricles (upper row), he corresponding Li-PDF 3D surfaces (middle row) and he discrepancy beween likelihood of paricles ha are obained by he Li-PDF and by direc measuremen-based calculaion (boom row) in differen sops (N=500, M=100, n=36)

35 35 TABLE II REAL-TIME PERFORMANCE OF PFS (SECOND) WHEN THE SCANNING DATA SIZE N=36 Number of paricles Basic PF Li-PDF PF TABLE III REAL-TIME PERFORMANCE OF PFS (SECOND) WHEN THE SCANNING DATA SIZE N=180 Number of paricles Basic PF Li-PDF PF V. CONCLUSION In his paper, a numerical fiing approach is presened o consruc he likelihood PDF (Li-PDF) of paricles for weigh updaing. I has highly improved he processing speed of paricle filers, especially for a broad range of Bayesian filering problems in which he measuremen updaing is much more compuaionally expensive han evolving he sysem dynamic. Our approach o a grea exen reduces he criical conradicion beween he compuaional cos and he sample-approximaion abiliy in paricle filers. Boh robo localizaion simulaions and visual racking experimens demonsraed ha he processing speed of he paricle filer is highly acceleraed by he Li-PDF approach wihou obviously losing esimaion accuracy.

36 36 APPENDIX A LINEARIZATION ERROR OF LI-PDF For engineering convenience, one may direcly assume he Li-PDF funcion having linear dependence on he parameers as in (19). However, he assumpion of he lineariy dependence does no hold for mos pracical sysems. This appendix gives he linearizaion error of convering a fiing model ha has nonlinear dependence on parameer o be one ha does acually no. The analysis is based on Taylor series expansion. A Taylor series is a series expansion of a funcion abou a poin. A one-dimensional Taylor series of a real funcion f(x) abou a poin x=x 0 is given by f x ( n) f f n f x0 f x0 x x0 x x0... x x0 R! n! n (A.1) where R n is a remainder erm known as he Lagrange remainder, which is given by ( n1) * f x Rn x x n 1! 0 n1 (A.) where x* [x 0, x] lies somewhere in he inerval from x 0 o x. Thus, if we use he engineering-friendly monomials funcion as in (19), here is a leas a sysemaic error of R n occurring. We can call i he Sysem Truncaion Error. The expression R n in (A.) indicaes ha, he closer he predicion daa x is wih x 0 (he smaller (xx 0 ) is), he more feasible i is o express he funcion in ha inerval in lower order and wih a smaller R n. Tha is o say, he closer he paricle is o he fulcrums (i.e. he smaller he piecewise inerval), he more accurae and robus he Li-PDF approach will be. This explains why he piecewise fiing is highly suggesed in our approach o deal wih he rade-off beween smaller piecewise inervals and higher compuaional requiremen.

37 37 APPENDIX B Goodness-of-fi (Gof) CHI-SQUARE es The probabiliy densiy funcion of he Chi-Square disribuion wih v degrees of freedom is f 1 v / / v / e 1 v/ (B1) where Γ(α) is he gamma funcion, defined as 1 x x e dx for 0 (B) 0 One way in which a measure of Goodness-of-fi saisic can be consruced, in he case where he variance of he measuremen error is known and he errors can be assumed o have a normal disribuion, is o consruc a weighed sum of squared errors O E (B3) where σ is he known variance of he observaion likelihood, O is he acual measure (supposed as paricles' fied value of Li-PDF) and E is he expeced measure (he direc observaion likelihood). As usually he σ is unknown, i would be esimaed by he Mean Square Error (MSE) N 1 MSE d (B4) v i 1 i where ν is he degrees of freedom. For our case, ν can be given by N k 1, N is he number of paricles adoped for es, and k is he number of fied parameers, assuming ha he mean value is an addiional fied parameer. To normalize he Chi-Square for he number of daa poins and model complexiy, he reduced Chi- Square saisic is divided by he number of degrees of freedom 1 O E (B5) v v red As a rule of humb, a large χ red indicaes a poor model fi. χ red=1 indicaes ha he exen of he mach beween observaions and esimaes is in accord wih he error variance.