A Fast JPDA-IMM-PF based DFS Algorithm for Tracking Highly Maneuvering Targets

Size: px

Start display at page:

Download "A Fast JPDA-IMM-PF based DFS Algorithm for Tracking Highly Maneuvering Targets"

Arabella Easter Young
5 years ago
Views:

1 A Fat JPDA-IMM-PF baed DFS Agorthm for rackng Hghy Maneuverng arget Mohand Saïd DJOUADI Yacne Mory Daoud BERKAI Laboratore robotue & roductue Ecoe mtare oytechnue B :7 Bord E Bahr 6 Ager Agére. Ema: mdouad@yahoo.fr Laboratore robotue & roductue Ecoe mtare oytechnue B :7 Bord E Bahr 6 Ager Agére. Ema: ymory@yahoo.fr Eectrca & Comuter Engneerng Ecoe atonae Poytechnue Avenue Haen Bad BP E Harrach Ager Agére. Ema: dberkan@hotma.com Abtract In th aer we reent an nteretng fterng agorthm to erform accurate etmaton n um Markov nonnear ytem n cae of mut-target trackng. Wth th aer we am to contrbute n ovng the robem of modebaed body moton etmaton by ung data comng from vua enor. he Interactng Mute Mode (IMM agorthm ecay degned to track accuratey target whoe tate and/or meaurement (aumed to be near mode change durng moton tranton. However when thee mode are nonnear the IMM agorthm mut be modfed n order to guarantee an accurate track. In order to dea wth th robem the IMM agorthm wa combned wth the Uncented Kaman Fter (UKF [6]. Even f the ater agorthm roved t effcacy n nonnear mode cae; t reent a erou drawback n cae of non Gauan noe. o dea wth th robem we rooe to ubttute the UKF wth the Partce Fter (PF. o overcome the robem of data aocaton we rooe the ue of an acceerated JPDA aroach baed on the deth frt earch (DFS technue [2]. he derved agorthm from the combnaton of the IMM- PF agorthm and the DFS-JPDA aroach noted DFS-JPDA- IMM-PF. Index erm Etmaton Kaman fterng Partce fterng JPDA Mut-arget rackng Vua ervong data aocaton. I. IRODUCIO h aer hoe to be a contrbuton wthn the fed of vua-baed contro of robot eecay n vua-baed trackng [3]; trackng maneuvrng target whch may themeve be robot a comex robem to enure a good track when the target wtche abruty from a moton mode to another not evdent. Becaue of the comexty and dffcuty of the robem a me cae condered. he tudy retrcted to 2-D moton of a ont whoe oton gven at amng ntant n term of t Cartean coordnate. h ont may be the center of gravty of the roecton of an obect nto a camera ane or the reut of the ocaaton of a mobe robot movng on a anar ground. Severa of maneuverng target trackng agorthm are deveoed. Among them the nteractng mute mode (IMM method baed on the otma Kaman fter yed good erformance wth effcent comutaton eecay when the meaurement and tate mode are near wth Gauan noe. However f the ater are nonnear and/or non Gauan noe the tandard Kaman fter houd be ubttuted n our tudy we chooe the Partce Fter (PF. he agorthm derved from th combnaton caed IMM-PF. he other robem treated n th aer about the data aocaton. Effectvey at each ame tme the enor (camera reent evera meaure and obervaton comng from dfferent target; the robem how to affect each meaure to the correct target to dea wth th robem we chooe an acceerated veron of the JPDA agorthm baed on the deth frt earch (DFS. he agorthm derved from the combnaton of DFS-JPDA and the non near IMM agorthm noted DFS- JPDA-IMM-PF. he aer organzed a foow. In ecton II the mathematca formuaton of 2-D moton reented. In ecton III we decrbe the IMM agorthm PF baed. In ecton IV we reent the DFS agorthm and than n ecton V we reent DFS-JPDA-IMM-PF agorthm. In ecton VI we reent and dcu the reut of muaton. Fnay n ecton VII we draw the concuon. II. MAHEMAICAL FORMULAIO OF 2-D MOIO he mathematca formuaton of 2-D moton ued many nred from Dane Douad and a n [4]. hey make the hyothe that the meaurement are ony the 2-D Cartean coordnate of the movng ont. Let (. denote the curvnear abca of M over tme onto t traectory the orgn of curvnear abcae et arbtrary. Functon x(. and y(. rereent the Cartean coordnate of M. he meaurement euaton may be wrtten a: x ( t = h( ( t ( t ( y( t Where (. a arameter vector functon of mnma ze. We can ee that euaton ( ndeendent of the tye of the moton of M onto t traectory. he tate euaton coud be wrtten a: X & t = AX t (2 ( (

2 ( t wth X ( t = ( t A A eua wth A the n x n zero matrx wth one added on t frt uer dagona and the matrce of convenent ze. he contnuou tme tate euaton (2 near tme nvarant and ndeendent of M traectory excet on the ze of (. and (.. Moreover t may be hown that the fundamenta matrx F nvoved t exact dcretzaton at the erod take the form n ( A F=ex( A =. =! he dynamc and meaurement noe are uoed to be tatonary whte and Gauan non nter-correated wth known covarance. A. Canonca moton euaton he ont M uoed to move on traght or crcuar traectore at contant or unformy tme-varyng eed (contant eed or contant acceeraton. hoe moton beong to the et of the obe behavour of a nonhoonomc robot whoe whee are drven at contant veocte or acceeraton. Outut euaton: One mnma decrton of a traght ne defned by the vector = ( α d hown n fgure (a whch reated to Pucker coordnate. Concernng a crcuar traectory one mnma decrton defned by the Τ vector = ( R x y hown n fgure (b. he orgn of curvnear abca unuey defned from thoe arameterzaton. M y + = Drecton of ncreang x R (b Crce Fg.. raectory Parameterzaton he outut euaton are a foow (traectory arameter are condered tme-nvarant: Straght Lne: x( d co + ( n z = α α = + v( (3 y( d nα ( coα ( Crce: x x + Rco z = = R + v( (4 y ( y Rn + R wth ( dtance covered by the target and v ( meaurement noe wth denty d ν( (ν. 2 State Euaton Contant veocty ( k + = k + = Contant acceeraton ( + w ( + w (5 d α = M (a Lne Drecton of ncreang 2 2 ( k + = ( + w (6 ( k + = + w where the random vector x ( w ( = ( w ( ( ( ( ( = w and ( ( = & && ( ( k vector of the ont M dynamc raectory arameter vector

3 III. HE IMM-PARICLE FILER ALGORIHM he bac dea to combne the IMM aroach [] wth a artce fter one. In the dervaton of the tandard IMM fter a mergng and fterng roce are defned. We adot a reguarzed artce fter for th fterng te and erform the mergng te on the robabty dente rereented by a Gauan mxture. One coneuence of the dcrete nature of the aroxmaton of the a oteror denty that t cannot drecty be aed to an IMM framework a t ued n []. o obtan a good contnuou aroxmaton of the a oteror denty we ue a reguarzed veron of the boottra fter a frt reorted n [89] for trackng target n cutter. In th hybrd veron of the boottra fter the robabty denty functon that ha been comuted a a ont ma robabty denty on a number of grd ont n the tate ace ftted to a contnuou robabty denty functon that a um of a refxed number of Gauan denty functon. Moreover by ung a hybrd tye of amng fter a an aternatve for drect reamng degeneracy n the effectve number of artce avoded [89]. he man advantage of the new method that we rooe here are: Agorthm the method abe to dea wth nonnearte and non- Gauan noe n a mode; the method ue a fxed number of artce n each mode ndeendent of the mode robabty. Let a ytem be decrbed by the euaton: x = f[x( k k ] + w[ k ] (7 z = h[x k ] + v[ k ] he roce noe and the meaurement noe are oby mode-deendent. her dente are denoted by: d w[k] (w and d ν[k] (ν. Where denote the mode at tme k. It a fnte tate r Markov roce tackng vaue n{ M } accordng to a = Markov tranton robabty matrx aumed to be known. he robabty denty of the nta tate known x( P (x. Defne the nformaton u to and ncudng tme te k a: Z = { z(... z } he fterng robem that ha to be oved : Gven a reazaton of Z( aocated wth (7 comute (x( Z(;.e. the condtona robabty denty of the tate x( gven the et of meaurement Z(. A cyce of the IMM agorthm coud be ummarzed n four te: Interacton tage Comute Mxng robabte µ ( k k = µ ( k (8 c Comute ormazng factor M ( k c = µ (9 Comute A ror robabty denty n mode ˆ ( x ( k Z ( k = ˆ ( x ( k Z ( k * µ ( k k M Fterng tage ( M draw ame x ( k accordng to x k Z k ( ( ( ˆ he redcted ame are: xˆ ( + w ( k f x ( k k = ( Where w ( k are ame obtaned from d ( ( w he redcted outut z ( k ( k k = h xˆ he robabty weght ormazng w k (2 ( = d ( z zˆ ( k k ν (3 k ~ (4 = = ormazed robabty mae = (5 ~ Mean of the tate over the ame et x = x = Covarance of the tate over the ame et ˆ (6 ( ( xˆ x ˆ = xˆ x (7 = P From the condtona robabty denty functon for the tate n mode baed on a mxture of Gauan dente ( ( x Z = xˆ Pˆ Pˆ ν (8 =

4 d 2 Whereν =.5 and d the dmenon of the tate ace. We obtan the robabty denty functon for the tate n mode after mxture reducton.e. baed on a mxture of r Gauan dente. ( ( ( ( = r r r r x k Z k xˆ Pˆ ˆ ν (9 = he mean of redcted outut over the ame et h ( k = h x = Redua covarance over the ame et Sˆ = h xˆ ˆ (2 ( ( k h ( h( xˆ k h = Innovaton ( k ( k = z( k h xˆ ( k (2 γ (22 Probabty denty functon for the nnovaton ˆ ( γ Z Sˆ Lkehood L Mode robabte Where Combnaton tage = = γ ; Sˆ ( = ( Sˆ ( (23 = (24 µ = L c (25 c M c = L (26 c he a oteror condtona robabty denty functon for the tae ( ( x Z = ˆ x Z ˆ µ (27 M IV. DFS ALGORIHM Let m and n be the number of meaurement and target reectvey n a artcuar cuter. he comutatona cot of data aocaton ncreae exonentay wth m and n. he effcency of the agorthm ued n the generaton of the data aocaton hyothe artcuary mortant when m and n are arge. In order to deveo an effcent agorthm to generate a data aocaton hyothee a mathematca mode deveoed for data aocaton. One of the mot ued mode for a combnatona robem caed exhautve earch wth contrant [3]. In the context of trackng muttarget data aocaton can be modeed a an exhautve earch wth a et of roer notaton. Let X ( = 2... m denote meaurement. he vaue of X beong to a et Z. he vaue of X dentfe the target whch hyothezed to be aocated wth meaurement. For exame X = 2 me that meaurement hyothezed to be aocated wth. here the et Z defned by: { t w = } = 2... m and t 2... n. Z = t = Where w t take two vaue f meaurement aocated to target t ee. Baed on the vadaton matrx Ω( w data aocaton hyothee [4] are generated ubect to two retrcton: ( each meaurement can have ony one orgn and (2 no more than one meaurement orgnate from a target. In a JPDA cenaro the above two contrant can be eay tranated nto the anguage of exhautve earch robem for data aocaton. Uuay an m-tue ( X X... 2 X... X... X m a outon the thee two contrant are atfed:. If X and X then X X 2. If X = X and then X =X =. A data aocaton hyothee can be generated by ovng the exhautve earch robem condered above. Here we ue the ecazed DFS agorthm rooed n [2] for the generaton of data aocaton hyothee. In genera n exhautve earch robem no outon are known n advance. However n the robem of data aocaton a outon whch away known (. he other outon can be generated ytematcay from varou vad combnaton of non-zero vaue of the eement. For more recon ee [2]. V. DFS-JPDA-IMM-PF ALGORIHM he rnce of the JPDA agorthm the comutaton of robabte aocaton for each track and new meaurement. hee robabte are then ued a weghtng coeffcent n the formaton of the averaged tate etmate whch ued for udatng each track. For a better decrton of the JPDA agorthm ee [25]. he combnaton of the JPDA and the IMM-PF agorthm done a foow. A nge et of vadated meaurement for JPDA-IMM-PF obtaned by conderng the nterecton Z k of r et of meaurement correondng to ndvdua mode: r Z k = I Z = k t

Where Z k rereent the et of vadated meaurement under the aumton that mode effectve. he combned kehood functon for the r mode of the IMM-PF agorthm are comuted a n [6].

5 Where Z k rereent the et of vadated meaurement under the aumton that mode effectve. he combned kehood functon for the r mode of the IMM-PF agorthm are comuted a n [6]. he ror mxed tate etmate for mode and the vadaton regon for ndvdua mode are ao comuted a n [26]. he new mode robabte outut tate etmate and correondng error covarance are obtaned a n [26]. VI. SIMULAIOS AD RESULS In th ecton we erform ome muaton to evauate our agorthm (DFS-JPDA-IMM-PF. he moton mode condered are: - contant veocty on traght ne (M -contant acceeraton on traght ne (M 2 - contant veocty on crce (M 3 - contant acceeraton on crce (M 4. o exore the caabty of our JPDA-IMM-UKF agorthm to track maneuvrng target varou cenaro are condered; among of them we eect the tyca cae of three hghy maneuvrng target wth crong traectore. We aume that the target n a 2-D ace and t oton amed every =. we run the DFS-JPDA-IMM-PF wth ame n each mode. - he robabty tranton matrx of four mode = he nta robabty of eectng a mode.25 that to ay at the tart a mode have the ame chance to be eected. - he curvnear abca (. reman contnuou even f a traectory um occur. A. Condered cenaro We conder that we have to track mutaneouy three maneuvrng target. In order to comcate the cenaro we uoe that the target foow durng there movement crong traectore. a arget (bac: he target tart movng accordng to mode M unt the 5 th ame when an abrut traectory change occur and t movng accordng to th durng 5 ame (wtchng from mode M to M 3. b arget 2 (bue: he target tart movng accordng to mode M 3 unt the 5 th ame when an abrut acceeraton about.2 m/ 2 occur and t movng accordng to th durng 5 ame (wtchng from mode M 3 to M 4. c arget 3 (green: he target tart movng accordng to mode M unt the 5 th ame when an abrut acceeraton about.2 m/ 2 occur and t movng accordng to th durng 5 ame (wtchng from mode M to M 2. d arget 4 (red: A the the tart movng accordng to mode M unt the 5 th ame when an abrut acceeraton about.2 m/ 2 occur and t movng accordng to th durng 5 ame (wtchng from mode M to M 2. y ax mode robabte of target mode robabte of x ax Fg.2. Rea and Eteemed raectore M Fg.3. Mode Probabte for target M Fg.4. Mode Probabte 2

6 mode robabte of M reut confrmed by the fgure ( from th we can ay that the tracker baed IMM-PF agorthm a ertnent outon to the robem of vua-baed trackng hghy maneuverng target. In the Other hand fgure 2 how ao that the data aocaton correcty done even f the traectore cro each other. h houd ermt u to ay that the JPDA agorthm comute erfecty and t combnaton wth the IMM-PF agorthm (DFS-JPDA-IMM-PF woud be an effcent outon to the robem of hghy maneuverng mut-target vua-baed trackng. RM S Poton Error n x coordnate 5 5 mode robabte of Fg.5. Mode Probabte 3 M RMS Seed Error Fg.6. Mode Probabte 4 target RM S Poton Error n y coordnate target Fg.7. RMS x and y oton error target RMS Acceeraton Error target Fg.8. RMS Acceeraton and Seed Error B. Reut nterretaton: Fgure 2 how that the eteemed and the rea traectory for the three target are ueroabe and amot dentca even f an abrut change occur on the tracked target dynamc. h VII. COCLUSIO he mode-baed body moton etmaton by ung data comng from vua enor t an oen robem on whch we try to rovde a contrbuton. In th aer we reented a nonnear agorthm whch attemt to track effcenty a hghy maneuverng target whoe traectory and/or dynamc coud change abruty and the noe dtrbuton not neceary Gauan; the agorthm rooed noted IMM-PF. o extend th agorthm to mut-target cae we combned the ater wth a fat veron of the JPDA agorthm noted DFS- JPDA to enure good data aocaton. Smuaton how that the DFS-JPDA-IMM-PF a good nvetment whe we are aked to track a hghy maneuvrabe target whoe meaurement and/or tate mode reent a trong nonnearte and the noe are not Gauan and when there dfferent traectore cro each other. REFERECES [] Y.Bar-Shaom and.e.fortman rackng and Data Aocaton Mathematc n Scence and Engneerng voume 79 Academc Pre 988. [2] Y.Bar-Shaom and X. R. L Etmaton and rackng Prnce echnue and Software Artech Houe Boton MA (USA 993. [3] B.Eau F.Chaumette and P.Rve A new aroach to vua ervong n robotc IEEE ranacton on Robotc and Automaton 8(3: June 992. [4] P.Dane M.S.Douad D.Beot A 2-D Pont-We Moton Etmaton Scheme for Vua-Baed Robotc ak 7 th Internatona Symoum on Integent Robotc Sytem (SIRS 99 Combra (Portuga 2-23 Juy [5] Y.Bar-Shaom and X. R. L Mut-target Mut-enor rackng: Prnce and echnue Storr C YBS Pubhng 995. [6] M.S.Douad A.Sebbagh D.Berkan IMM-UKF and IMM-EKF Agorthm for rackng Hghy Maneuvrng arget Archve of Contro Scence Vo: ue: 25. [7] M. Hadzagc H. Mchaka A. Jouan " IMM-JVC and IMM-JPDA for coey maneuverng target" IEEE 2. [8] Y. Boer J. Dreen " Interactng mute mode artce fter" IEE Proc- Radar Sonar avg Vo.5 o. 5 October 23. [9] S. McGnnty G.W. Irwn "Mute mode boottra fter for maneuverng target trackng" IEEE ran. Aero. Eectron. Syt.2 36 ( [] S. McGnnty G.W. Irwn "Manoeverng target trackng ung a mute-mode boottra fter" n Doucet A. de Freta. and Gordon. (Ed.: Seuenta Monte Caro method n ractce (Srnger ew York [].J. Gordon"A hybrd boottra fter for target trackng n cutter" IEEE ran. Aero. Eectron Syt ( [2] B.Zhou.K. Boe " Muttarget trackng n cutter: Fat Agorthm for Data Aocaton" IEEE tran. Aero. Eect. Vo.29.2 Ar 993. [3] E.M. Rengod J. everget. Deo " Combnatona Agorthm heory and Practce" Prentce-Ha 977. [4].E. Fortmann Y. Bar-Shaom M. Scheffe "Sonar trackng of mute target ung ont robabtc data aocaton" IEEE Journa of Oceanc Engneerng OE-8 Juy 983.

JPDA-IMM based Particle Filter Algorithm for Tracking Highly Maneuvering Targets

JPDA-IMM based Particle Filter Algorithm for Tracking Highly Maneuvering Targets JPDA-IMM baed Partce Fter Agorthm for rackng Hghy Maneuverng arget Mohand Saïd DJOUADI Yacne Mory Daoud BERKAI Laboratore robotue & roductue Ecoe mtare oytechnue B :7 Bord E Bahr 6 Ager Agére. Ema: mdouad@yahoo.fr