Application of UK-GMPHDF algorithm based on IMM in multiple maneuvering targets tracking

Transcript

1 H 3 KH μ w_r`s>>+ Vol.3 No. 2f E Systems Engneerng Theory & Practce Nov. 2 prff±: fdb^±: TN953; TN96 p~ `u: A #T IMM Ω UK-GMPHDF? Xff$f6 νb_ωqr ffla2 6~h 2 I C. f mq_ qv»_j f 5; 2. N"ffflc?`'J N" 33 Y O S!* Z^WQ4js Z^ yοu;f±rtm Lnbnνff y: IMM suß+ 5DYsΠGμ Ξ3ffB7w4j UK-GMPHDF H}. ΛH}ψ IMM H} xp;fοu: spee=. UK-GMPHD 4j%wΠ ρh[sgt. or 4j> -f UK-GMPHD 4j #e8<d!*sfψ/tm z@d/ Φρ;fF ;f ok. l Z^WQ j &X Offxyοu;f±rsef m flλh} νr: s UK-GMPHDF H}$]dχ mψωg9ν IMM s UK-GMPHDF H}hχ s± r^= pplπyοu;f±r%w fa±rsy;fvl. ρφ οu;f±r; ΠGμ Ξ3ffB7w4j; Uß+ 54j; ff y: ; Z^: Applcaton of UK-GMPHDF algorthm based on IMM n multple maneuverng targets tracng HAO Yan-lng MENG Fan-bn 2 SUN Feng SHEN Feng. College of Automaton Harbn Engneerng Unversty Harbn 5 Chna; 2. Tanjn Navgaton Instrument Research Insttute Tanjn 33 Chna Abstract For the purpose of solvng the problem of nonlnear flterng and the multple maneuverng targets tracng a novel unscented Kalman mplementatons of the Gaussan mxture probablty hypothess densty flter UK-GMPHDF based on nteractng multple model IMM s presented. The adaptve ablty to varous target maneuverng models s combned wth the advantage of hgher accuracy and lower computaton load provded by UK-GMPHDF. Furthermore the UK-GMPHDF avods the data assocaton problem and s able to jontly estmate the tme-varyng number and ther states. Under nonlnear system and clutter envronment the proposed algorthm s compared wth constant turn CT model and current statstcal CS model based on UK-GMPHDF n maneuverng target tracng. The expermental results show that ths novel algorthm can sgnfcantly mprove the tracng performance and reduce the multtarget mss-dstance. Keywords maneuverng target tracng; Gaussan mxture probablty hypothess densty flter; unscented Kalman flter; nteractng multple model; nonlnear model PM KRοMΠ ovm@e Π AKΠ οm=t>οmal Em]AΦbfflA RΠ 3K ff! ßψhqΩ< 3KοAΦψs" ^=$$3K` qz. χdrοmπ ovaffryi:<a&a-.. Πρ+:f]Y -flr - fψhqkhwf IMM-JPDA flr /R#6ov IMM/ MHT ARY [ 2] T% *EΠροMΠ ovχdm@e3:ffary :E UrIμK Π K AO"@@χR yψ±p BO% 3K ffwf4sflrοmπ ov?peqz djν KοMΠ ovea26 [ 3]. Locheed Martn A Mahler ΦΛVΦ6ffA Vo C8 AffS c6 <μ98: 29-- a`j5: ~!u&rfν; 6748 cz-: Ξ" 944 ff 2G νd9>: ψe^ : vω yμ>λ; [» 98 Φ νdψe9 ψe^ : vω S± pw.

2 2226 x`s*at?? H 3 K Uοf RFS `safhq#6}p & PHD 'EUοffflA5_χ ]P<UοfflA5 _χ Q PHD ρaff@?π KA j_ Yψ PHD Ac_nc lπ ApWuχ_ rμ] Yb»j_AKHwf]Y fl~jkhwfψz [4 7] 66/EjχR Tf8J@ PHD rμ]y' RΠ ovm@χda'j :fl69 JΠ HM <;g >Π HM h# ffl= Q ovφuχfez8a Q<οMAΠ [M νhm U=OSfffflff. 9 vla ffψq$ JΠ AHMpW KοMΠ oveοjf6. μ< Marov o hqaflr IMM RYI:t KY χrfe ovff gπax ν R ]YA7-8 D[DH? Az wn [ 3]. VQ2qZ K` jn -ΦIμfflCy Y-jμ< IMM ArffNVv> AjN Ψhq #6}Prμ UK-GMPHDF RY. TEhffArμuχRY c6 IMM XΠ AHMXX νοh Π οml A d QΠ HM F2 3ff; rμß UK-GMPHDF rffffl UT μ/ fl6n Vv rμw%> A GMPHDF RYE PHDF AhfB9t9I B c67:"_ M_Φ] "AjNaA""ΦΨ>f#}P K. ΦoVY-Aμ< IMM A UK-GMPHDF RYfftYjΠ b WοM=AovffeΦov?P _U8 #[jtrya:ff D%! rffnvvrμ UKF Erffffl UT Φ qnvvrμ[wa8ψ >ZQNVvrμ EKF ο ^ UKFE] w_]f6< -ya qnvvrμ[w UοEΠ EKF % ] ff` K> Ierμ. Q<ff` jnw_ gryuχ?pcj< EKF χrcω<d srμ. UT E UKF ]YA fflφμ/ EhχRUοffl@ ` ffl f_χx A]Y [8 9].UT 9:4-y. #LyΠ e2` jnaß Π pw]φxψ]: x = φ x v z = h x ε νe x E =T n lπ pw z E =T m lxψ φ h ` K w_m7 v Φxψ M7 ε a fl]" Q Φ R AjNΛM7. c6 =TAG l Sgma JAZlAM_ µ l { } flfl] 2n " C l #8 2n + Sgma J y l ΦQ2Ay"_ { u l} 2n l= l= µl Cl Φ yl C: [ µ l = m l T T] T 2 C l =dag P l Q R 3 y l =TA Sgma J y l Φ"_ ul a 5 BΦ 6 B: y l y l y l [ T T ] T T = x l v l ε l 4 = µl l = = µl + n + λ C l l = 2 n l = µl n + λ C l l = n + 2n l u l = λ/ n + λ l = u l = λ/ n + λ+ α 2 + β l = 2 n u l = λ/ [2 n + λ] l = n + 2n νe x l ml Φ P l a C =TG l Sgma JApW M_Φfl]". n + λ C l EGW n + λ C l ]nag l»; λ = α2 n + κ n C+Ps α LL Sgma Jf?M_JAJ_ 6Ω $ψffωazk -. κ $ψ β 6[ JUοfflAaß u QjNaß β =2= z8 [8]. 5 6 l

3 H μ Ξ" D: ν= IMM B UK-GMPHDF SZLSßN± pwfb UK-GMPHDF ffb Pp UKF RYAmR8J. UKF > GMPHDF RY8Ψο[ #8q` Arμ ff [7 ] y m-j UK-GMPHDF RYAhYflffl AIeρl. UK-GMPHDF Aψ #L =TA PHD K D x jnφ B: D x = J = w N x; m P waψ PHD K D x 'jnφ B: D x =D S x+d B x+γ x 8 νe w m Φ P a =TAjNa A"_ M_Φ]" J C =TAj NaxK. νe #L =TΠ 3»hq P S 3»Π A PHD K D S : J D S x =P S!8Π A PHD K D B : D B x = ff8π A PHD K γ : J = J B j= = w S N x; m S P S w wj B N x; m j B P j B 7 9 m j = m + mj B P j = P + P j B 2 J γ γ x = = w γ N x; m γ P γ Q< l = 2n ] UT χr` pw]apw: x l = φ x l vl νρaψm_φ]"a : P = L l= zf K =TAψ PHD K: 2n m = l= 3 4 u l x l 5 T u l x l m x l m + Q 6 D x = J = w N x; m P 2 UK-GMPHDF qff Q- l = 2n ] UT χr` xψ]aρaψ z l z l = h x l εl νρaψm_φ]"a : S = 2n l= 2n η = l= : 7 8 u l z l 9 T u l z l η z l η + R 2

4 2228 x`s*at?? H 3 K 92ρ+ff- PHD K D x AjNΦ B =TAqff PHD K D x : D x = P D D x+ D D x; z 2 z Z νe P D C =TAψhq D D x; z A 5B: D D x; z = J = jna AuχM_Φ]"a : m P P D w q z N x; m J κ z+p D j= w j q z =N z; η S z =m + K = P G [ S qj z η ] [ G z P z ] T 25 νe pwo/gwφfl]"a : K = G [ S ] 26 3 ν*>ψ G = 2n l= T u l x l m z l m 27 9<f#hq}PAjNΞU='r AO" judjnξkπ 6cν*}_ τ!."_ PHD ee<gz AjNΞ. nf 6cΨ }_ U ff4>ajna=»ψ Q<ν*>Ψ > fff o} [4] Φ [2]. 4 pwuχ YrμRYFE] 2Ie h'a wπ Kuχ+ΨuχA"_^Φ: J N = w 28 N z6"_q2ajnay'afψ Π ApWuχ [7]. 3 7 Wfl4LχfΠ4L = ρψπ Anc QPfl"QPKF` wa 3 Aa4a pw± Π ApW x [ = x ẋ ẍ y ẏ ÿ z ż ] z x y ẋ z ẏ ẍ ż Φ ÿ z a CK =TAΠ Anc QPΦ"QP x s y s z s 5nc. Π HM ZlfA$Qqog CT Q<Π $Q ;ogqql= ff$q CV $Qqog HM E ffla ov?p3j U ;ρ _χ CS QjPοMΠ I:ffffAtF2ffeΦ3j A?P :Q/οMΦ`οMΠ ov?p Q&E [3]. Φo.2Jhh =»fl > jqπ A lhhm Off> 3j?PAov. 2 CT Q<;ρV6 r { 2 N r } ; r == j@ CT >6LApW±b Q CT ApWffl=»jZl ffo"j x y Φ z ]±2"QP 3 a. T% ZQfApW] FSa K_ApW±2=»flj CT ApW]: x = F x + w 29 sn ΩT A 3 3 F Ω cos T Ω = 3 A 3 A = sn ΩT cosωt Ω 3 3 A Ω sn T cos ΩT

5 H μ Ξ" D: ν= IMM B UK-GMPHDF SZLSßN± pwfb G 3 3 G = 3 G G G = 4 T 4 2 T 3 2 T 2 2 T 3 2 T 2 T 2 2 T 2 T 2 T νe F C r = -y K =TΠ ApWo*GW Ω ogqq w C =TΠ A7:]"GW G AlM_jN± T fl%j 3 C 3 3 AlGW. 3 CS ; r =2= CS ApW]: x = F 2 x + U ᾱ + w 2 3 A T F α +αt + e αt 2 = 2 3 A 2 3 A 2 = α e αt 3 3 A 2 e αt [ α T + α 2 T 2 + ] α e αt U = T G α e αt Q 2 =2ασ2 a 3 G 2 3 e αt 3 3 G 2 q q 2 q 3 G 2 = q 2 q 22 q 23 q = [ 2α 5 e 2αT +2αT + 2α3 T 3 ] 2α 2 T 2 4αT e αt 3 q 3 q 23 q 33 q 2 = [ e 2αT 2α 4 + 2e αt +2αT e αt 2αT + α 2 T 2] q 3 = [ e 2αT 2α 3 2αT e αt ] q 22 = [ 4e αt 2α 3 3 2e 2αT +2αT ] q 23 = [ e 2αT 2α 2 + 2e αt ] q 33 = [ e 2αT ] 2α 2. νe F 2 r =2= K =Π ApWo*GW U UdGW α Π οm q ffοm= 'A<K ᾱ οm"qp ;ρ M_ w 2 lm_]" Q 2 AjNΛM7 σa 2 "QP]". 4 xψ =Tß 5 AxψΠ A± z = [ θ ϕ r ] θ ϕ Φ r a ßxψΠ A]n f$φj_. 9ßxψAΠ Axψ -y: θ arctan y ys x xs ϕ z = arctan zs x r xs2 +y x ys2 + x 2 s + y 2 y s + z 2 z s νe v θ v ϕ Φ v r a EM_l ]" σ2 θ σ2 ϕ Φ σ2 r AjNΛM7. 4 vθ vϕ vr 3 IMM AB`E.w_AHM 55 f μ<y Arμß»r c6yrμßi- Aχ" ufl Az# u HΞh#6# ;ρ=tw_ ffl Ahq hq w_apwuχel rμßuχahq""+ψ [3]. jyjοmπ Aov?P Tb. IMM > UK-GMPHDF hhry 8Ψ c6thhry=- hμ< IMM A UK-GMPHDF ovrμry. IMM rμ ΠYI.fl UK-GMPHDF rμ hqqffφuχ+ψoffa -c VC. c e w p m p P p =TG p rμßa I- p = 2 N r N r xk w op m op P op w p m p P p fl 6A8 ν; =Trμß p AI.. w p m p Φ P p a =T p yrμß I-AjNa A"_ pwuχφfl]" w op m op Φ P op a =T p y fl 6AjNa A"_ pwuχφfl]". Λ p p AjNa AP% K µ p p y

6 223 x`s*at?? H 3 K jna A hq. Z =TAxψ rμßa+ψi-. w m P μ< N r μ/^2ajna A UK-GMPHDF UK-GMPHDF A "S IMM Ψ UK-GMPHDF >ffl+οa Q< UK-GMPHDF RY26+@:4 U UK-GMPHDF > A IMM ARYI[ρl-y: χr Ψhq #L =TA ffl E M p UK =TA ffl E M q KxψKH Z -y jn a A Ψhq: µ pq = P M q M p Z = c q π pq µ p 32 c q = π pq µ p 33 p= νe q = 2 N r c q ff$k π pq C Marov o*hq ff =TA M p? =TA M q Ao*hq µ p K =TjNa A M p - Ahq. 2 Afl M p Az# PHD K D p x w M q rμßfla PHD K D q x : N r D q x = D p xµ pq 34 p= M q yjna AA"_ pwφfl]"a : w q = w p µ pq 35 p= p= m q = m p µ pq 36 p= P q = [ ] T µ pq P p + m p m q m p m q. w q m q Φ P q =T M q AI. χr@?q2arμi- w q m q Φ P q. 3 P% K νe M q yjna AP% K: Λ q = p z M q Z = 2πS q 2 exp 2 2n S q = l= v q T S q v q T u l z l η z l η + R 39

7 H μ Ξ" D: ν= IMM B UK-GMPHDF SZLSßN± pwfb hqqff M q hqqff: µ q = P M q Z = c νe c jna A ff$k ff: 5 uχ+ψ q= v q = z η 4 N r Λ q p= q= π pq µ p q = 2 N r 4 N r c = Λ q c 42 xa PHD K9y Ψ]@-: N r D x = D q xµ q = J w q N x m q P q q= = jna AuχA"_ pwm_φpwfl]"a : P = q= w m = q= = q= [ µ q P q + m 43 w q µ q 44 m q µ q 45 m q m ] T m q 2ρlE UK-GMPHDF rμß> A IMM RYAI flffl gry+ψj UK-GMPHDF Φ IMM A8J. ovw_fftyjiμ }fπ Φ` AοMffe. 5 ]U G 5. [ψ. >#Kl &Uχ@ [ 4 m 4 m ] 3 Ω ß 5 n< m ψhq P D =.95 5AJ_xψ]" σ 2 r =6m 2 ]n f$]"m σ 2 θ = σ2 ϕ =mrad 2 fl%j T =s>#_uρ% 5 s K&Uχ@Ω: 7 Π AKο^=T- οmφψ; yπ A3»h q P S =.96. #Lx:!8Π ffπ - dψ Posson RFS A PHD K: 4 γ x =.Nx; m γ P γ 47 = IμA Posson RFS z =λ uz νeiμffp λ = 5 uz &ψχ@abaß * X_ τ = 5 Ψ z U =4Gßz6AjNa"K J max = 2. _Ue6Π Uο"QP]" σa 2 = 2m/s 2 2 CT AogQq Ω = π/rad M7 q" σ 2 w = 2rad/ s 2 2 CS Q2Az6"QPm/s 2 οm q α=.4 ;ρ z6"qp6l a max = [ ] 4m/s 'Az#hq µ =[.5.5] ^'A Marov o*gw: π =

8 2232 x`s*at?? H 3 K 5.2 :N»F c 2 CjU>Π K&Uχ@ΩAοM}ffΦxψ_ > lπ οmau>}ff xψ _. c 3 μ< IMM A UK-GMPHDF Auχ_ > U>}ff o lπ Auχnc SOgRYfft3ΠAovRοMΠ. jqπa#[ryas» Φ:ff Q CT CS Φ IMM ltarμuχ8 =»Ω3. c 4 μ< CT CS Φ IMM A UK-GMPHDF RYAΠ KA uχ_>>ψπ KAΩ3ff c SO- KY># e CT rμ3kffaπ ;oφso Φ CS μφ2fftovhmπ :Q<`οMΠ ov ff3" Kffa=T- so Φ -K =' t=3 32s Φ =42 5 s - Π K4suχ Ufl6 CT Φ CS =»fl QοM>`οM HMΠ O> j?paov 2AagMffZ$Auχ-Π K. c 5 m-jμ< CT CS Φ IMM A UK-GMPHDF RYARΠ z8s affl OSPA J_uχ8 OSPAJ_EΛVΦ6ffA Vo χdωyz>y-a $Π ov ffa s"j_ Q<RΠ s"j_hflfff [4 6]. IMM rμuχ OSPA J_KY># Q μφk m y Ω< CT Φ CS Ar μ8 fμ< CT Φ CS A UK-GMPHDF RYov fftff - W Φ Uμ< IMM A UK-GMPHDF RY:ffAfl~TffflXA- ov?p66yj. z/m x x y/m x/m 6 2 A 2 5 ;fl& ;I ffffξ^ ±.5 x 4 z/m x x x y/m x/m A 3 "S IMM Ψ UK-GMPHDF ß '^ V ff ;fl& ;I ]. UK-GMPHDF y ArμRY Y-jhμ< IMM A UK-GMPHDF RY ν I:b-8J: ο<fl~jkhwf 9Lj yψ±p qz UfSfΨuχRοMΠ KΦΠ pw; 2 IMMRYelrμ V»χR YjjχRffq ο O"χR; UKFRYHR='36< Kalman Φ EKF RY :QRY>= 4ΛffΩ; 3 KIμ ` fflcyaοmov ff:rffffasq ΦoΞ ; 4 fftqοm>`οmaπ j?paov. K` ΦIμfflCy _U8 >9 A UK-GMPHDF RY=»jΩ3 gry YjjbWοMΠ Aov ff I:3ΠA26ρB. 6 CT 5 CS IMM t/s A 4 "S IMM CT 3Kff CS 3KΨ UK-GMPHDF Ψ5 =ß'^~*

9 H μ Ξ" D: ν= IMM B UK-GMPHDF SZLSßN± pwfb IMM 35 CT CS 3 OSPA /m ΛCH t/s A 5 "S IMM CT 3Kff CS 3KΨ UK-GMPHDF Ψ OSPA E /~* [] L X R Vesseln P J. Survey of maneuverng target tracng part I: Multple model methods[j]. IEEE Transactons on Aerospace and Electronc Systems 25 44: [2] Lee B J Par J B Lee H J et al. Fuzzy-logc-based IMM algorthm for tracng a maneuverng target[c]//iee Proc Radar Sonar and Navgaton USA: IEE 25 52: [3] L X R Vesseln P J. Survey of maneuverng target tracng part I: Dynamc models[j]. IEEE Transactons on Aerospace and Electronc Systems S : [4] Vo B N Ma W K. The Gaussan mxture probablty hypothess densty flter[j]. IEEE Trans Sgn Proc 26 54: [5] Vo B T Vo B N Canton A. Analytc mplementatons of the cardnalzed probablty hypothess densty flter[c]//ieee Trans Sgn Proc 27. [6] Molchanov. Theory of Random Sets[M]. New Yor: Sprnger-Verlag 25. [7] Mahler R. Statstcal Multsource Multtarget Informaton Fuson[M]. Artech House 27. [8] Farna A Rstc B. Tracng a ballstc target: Comparson of several nonlnear flters[j]. IEEE Transactons on Aerospace and Electronc Systems S : [9] Juler S J Uhlmann J K. Unscented flterng and nonlnear estmaton[j]. Proc IEEE : [] Ito K Xong K. Gaussan flters for nonlnear flterng problems[j]. IEEE Trans AC 2 455: [] Sngh S Vo B N Baddeley A et al. Flters for spatal pont processes[j]. SIAM J Control and Optmzaton : [2] Ma W K Vo B N Sngh S et al. Tracng an unnown and tme varyng number of speaers usng TDOA measurements: A random fnte set approach[j]. IEEE Trans Sgn : [3] Hoffmann C Dang T. Cheap jont probablstc data assocaton flters n an nteractng multple model desgn[j]. Robotcs and Autonomous Systems 29 57: [4] Hoffman J Mahler R. Multtarget mss dstance va optmal assgnment[j]. IEEE Transactons on Aerospace and Electronc Systems Man and Cybernetcs-Part A : [5] Vo B T Vo B N. Performance of PHD based mult-target flters[c]//proc 9th Int Conf on Informaton Fuson Florence Italy 26: 3. [6] Schuhmacher D Vo B T Vo B N. On Performance evaluaton of mult-object flters[c]//ieee Trans Sgn Proc th Annual Conf Informaton Fuson Cologne Germany 28.