Implementation of the GIW-PHD filter

Transcription

1 Technical reort from Automatic Control at Linöings universitet Imlementation of the GIW-PHD filter Karl Granström, Umut Orguner Division of Automatic Control 28th March 2012 Reort no.: LiTH-ISY-R-3046 Address: Deartment of Electrical Engineering Linöings universitet SE Linöing, Seden WWW: htt://.control.isy.liu.se AUTOMATIC CONTROL REGLERTEKNIK LINKÖPINGS UNIVERSITET Technical reorts from the Automatic Control grou in Linöing are available from htt://.control.isy.liu.se/ublications.

2 Abstract This reort contains seudo-code for, and a comutational comlexity analysis of, the Gaussian inverse Wishart Probability Hyothesis Density lter. Keyords: Extended target, robability hyothesis density, random matrix, Gaussian, inverse Wishart, comutational comlexity.

3 Imlementation of the GIW-PHD lter Karl Granström, Umut Orguner Abstract This reort contains seudo-code for, and a comutational comlexity analysis of, the Gaussian inverse Wishart Probability Hyothesis Density lter. 1 Introduction This technical reort contains information regarding imlementation of the Gaussian inverse Wishart robability hyothesis density gi-hd lter [1]. Pseudo-code is given for the main lter recursion. The comlexity is analyzed in terms of the number of multilications and summations that are needed, and a orst case aroximate of the comlexity is given. Please refer to [1] for further details. 2 Pseudo-code The main lter recursion is given in Table 1, rediction and construction of correction comonents is given in Table 2, and correction is given in Table 3. The runing and merging scheme is given in Table 4, and target extraction is given in Table 5. Note that runing and merging, and target extraction, is erformed similarly to [2]. Table 1: Pseudo-code for the Gaussian inverse Wishart hd lter 1: inut: Sequence of measurement sets Z } K =1. 2: initialize: Set J 0 0 = 0. 3: for = 1,..., K do 4: Comute measurement set artition } P, see [1, 3]. 5: Predict and construct correction comonents, Table 2. 6: Correct, Table 3. 7: Prune and merge, Table 4. 8: Extract estimated target set, Table 5. 9: end for } K 10: outut: Sequence of estimated target sets ˆX =1 1

4 Table 2: Pseudo-code for gi-hd lter rediction and correction comonents 1: inut: gi comonents, and set of measurement set artitions } P. 2: i = 0 3: for j = 1,..., J b, do 4: i i + 1 5: i 1 j b,, 6: end for 7: for j = 1,..., J 1 1 do 8: i i + 1 j 1 1, ξj 1 1 ξi 1 ξj b, 9: i 1 S j : m i 1 F 1 I d m j : P i 1 F 1P j F T 1 + Q 1 12: ν i 1 e Ts/τ ν j 13: V i ν i 1 d 1 ν j : end for 15: J 1 i 16: for = 1,..., P do 17: for = 1,..., do 18: z W 1 W 19: Z W j d 1V 1 1 z i z i W 20: end for 21: end for 22: for j = 1,..., J 1 do 23: ˆKj 1 P j 1 H T 24: Ŝ j 1 H ˆK j 1 W z i z i 25: ẑ j 1 H I d m j 1 26: end for, and correction como- 27: outut: gi comonents } nents z W, Z W =1 } P zw z i j 1, ξj 1 and } J 1 1 T zw } J 1 ˆKj 1, Ŝj 1, ẑj 1 } J 1 2

5 Table 3: Pseudo-code for gi-hd lter correction 1: inut: gi comonents j 1, ξj 1 } P, and correction comonents z W ˆKj 1, Ŝj 1, ẑj 1 2: for j = 1,..., J 1 do 3: j 1 } J 1 1 e γj j D 4: ξ j ξj 1. 5: end for 6: l = 0 7: for = 1,..., P do 8: for = 1,..., do 9: l l : for j = 1,..., J 1 do. j 1 11: S Ŝj 1 + 1, W j K ˆK 1 S 1 12: ε z W ẑ j N S 1 εε T 13: m j+j 1l 14: P j+j 1l 15: ν j+j 1l 16: V j+j 1l 1, m j 1 + K I d ε P j 1 KSK T ν j 1 + W V j 1 + N + ZW 17: j+j 1l jγ e γ j W j D β W F A,π W W S d/2 18: end for 19: d W δ W,1 + J 1 j+j 1l 20: j+j 1l 21: end for j+j 1 l d W 22: ω =1 d W 23: end for 24: J J 1 l : J aux J 1 26: ω ω P for = 1,..., P =1 ω for j = 1,..., J 1 } J 1, artitions } } P, Z W =1 V j 1 νj 1 /2 V j+j l ν j+j l /2 27: for = 1,..., P do 28: j+j aux j+j aux ω, for j = 1,..., J 1 29: J aux J aux + J 1 30: end for 31: outut: gi comonents } j, J ξj and Γ d ν j+j l /2 Γ d ν j 1 /2 j 1 3

6 Table 4: Pseudo-code for gi-hd lter runing and merging } 1: inut: gi comonents j, J ξj, a truncation threshold T, a merging threshold U and a maximum alloable number of gi comonents J max. i 2: initialize: Set l 0 and I i = 1,..., J }. > T 3: reeat 4: l l + 1 5: j arg max i i I ˆP j 6: Comute, see [1]. 7: L i I 8: l i L i 9: m l 1 l 10: P l 1 l 11: ν l 1 l 12: Ṽ l 1 l 13: I I\L 14: until I = m i mj i L i mi i L i P i i L i νi i L i V i 15: If l > J max then relace T 1 } j ˆP m i mj U j, mj J max gi comonents ith largest eights. } l j j j, ξ, ξ = 16: outut: } l j, P, νj, Ṽ j m j j, P, νj, Ṽ j by those of the Table 5: Pseudo-code for gi-hd lter target extraction 1: inut: gi comonents 2: ˆX = 3: for j = 1,..., J do 4: if j > 0.5 then j 5: Comute ˆX, see [1]. 6: ˆX ˆX m j j, ξj, ˆX j 7: end if 8: end for 9: outut: Estimated set of targets ˆX } J. 4

7 3 Imlementation issues When comuting the corrected eight Table 3, Line 17, the lielihood often includes ratios of large numbers, leading to numerical overo. Because of this, comuting the log-lielihood is recommended, and then udating the eight ith the exonential of the log lielihood. Similarly, the quantities d W Table 3, Line 20 are often large, leading to numerical overo hen ω Table 3, Line 22 is comuted. A remedy is to store the log artition eights ω = log ω = log d W, 1 =1 and to normalize the log artition eights Table 3, Line 26 as follos, ˆω P = ω log ω =1 = ω ω 1 + log 1 + P =2 e ω ω 1 2a 2b for = 1,..., P. The artition eights are then given as ω = eˆω for = 1,..., P. 4 Comutational comlexity analysis 4.1 Comlexity of common oerations The comlexity of some common matrix and vector oerations is given in Table 6. In addition, comuting the inversion and the determinant of a d d matrix V both have aroximate comutational comlexity Od Assumtions and aroximations To simlify the notation, let J = J 1 1, J b = J b, and J + = J 1 = J J b,. To simlify the analysis, the folloing assumtions are made; Table 6: Comlexity of common oerations Inut Oeration Multilications Summations Comlexity Am n, Bn AB mn mn 1 m2n 1 Am n, B q A B mnq 0 mnq Am n, B q A BC mnqt + 1 mnq 1t m2nq 1t + nq Cnq t n A i q i=1 A i 0 n 1q n 1q x i d 1, y i d 1 x i y i 0 d d n z i d 1 i=1 z izi T nd 2 n 1d 2 2n 1d 2 F n n, P n n F P F T 2n 3 2n 3 n 2 4n 3 2n 2 5

8 1. The estimated target cardinality is aroximately correct, i.e. ˆNx, N x,. 2. The number of gi comonents are aroximately equal to the number of targets, i.e. J N x,. 3. The true target Poisson rate γ is equal for all targets. The number of measurements is N z, γn x, + λ, here λ is the mean number of clutter measurements. 4. After artitioning, each cell W ith target generated measurements has cardinality aroximately equal to the Poisson rate γ, i.e. W γ. 5. After the correction ste, there are aroximately N x, clusters ith gi comonents, and each cluster contains aroximately J + gi comonents. Thus, in the runing and merging ste, e have L J +, here L is the set of gi comonents that are merged into one comonent. 4.3 Prediction and construction of correction comonents The comlexity of rediction and construction of correction comonents is given in Table 7. Predicting each of the J comonents has aroximate comlexity OJ 3n 2 x + 4s 3 + d 2 n x s OJn 2 x. 3 Constructing the centroid measurements and scatter matrices has aroximate comlexity P γ2d O 2 + d d 2 + d + 2 P O γd 2. 4 Constructing the gain matrices, innovation factors and innovation vectors has aroximate comlexity O J + J b 3n x d + 2s 2 + s d 1 O J + J b n 2 x. 5 Table 7: Comlexity of rediction and correction comonents Oeration Multilications Summations Comlexity S F I d m s 2 d 2 + s 2 d 2 sdsd 1 3n 2 x n x F P F T + Q 2s 3 2s 2 s 1 + s 2 4s 3 s 2 e Ts/τ ν O3 ν d 1 ν d 1 V d d W z 2 γ 1d γ 1d + 2 z zz z T γd 2 2d + γ 1d 2 2γ 1d 2 + 2d P H s 2 ss 1 2s 2 s HK s s 1 2s 1 H I d m sd 2 + sd 2 dsd 1 3n x 1d 6

9 Thus, the overall comlexity of rediction and construction of correction comonents is aroximately O J + n 2 x + γd 2 P Correction The comlexity of correction is given in Table 8. The correction udate of the gi comonents has aroximate comlexity O P J + 3n x d + d 3 + 3d 2 + d + s + 6 O n 2 xj + P. 7 Comuting the cell eights δ W and artition eights ω has aroximate comlexity P P P O J O. 8 J + Normalizing the artition eights and udating the gi comonents eights has aroximate comlexity OP P OP. Thus, the overall comlexity of the correction is aroximately O n 2 xj + P. 9 Table 8: Comlexity of correction Oeration Multilications Summations Comlexity S + 1/ W KS s 0 s + 1 z z 0 d d S 1 εε T 1 + d d m + K I d ε sd 2 + sd 2 sdd 1 + n x 3n x d P KSK T s s 2 2s ν + W V + N + Z 0 2d 2 2d 2 eight udate Od 3 δ + 0 J J dw Jl ω ω 1 P 1 P ω J + J

10 4.5 Pruning and merging The comlexity of merging is given in Table 9. Determining hether or not comonents i and j should be merged has aroximate comlexity O nx 3. There are aroximately N x, J + comonents remaining after the correction ste. In the orst case, each comonent has to be comared to all other comonents, i.e. O Nx, 2 J + 2 comarisons. Thus, the orst case comlexity of the merging is aroximately O Nx,J 2 +n 2 3 x Target extraction Comuting the extension estimate ˆX has aroximate comlexity Od 2. Under the assumtion that the target cardinality estimate is aroximately correct, the comlexity of target extraction is aroximately 4.7 Partitioning the measurement set O N x, d For Distance Partitioning, creating the distance matrix requires 3N z, N z, 1 multilications and summations. The distance matrix must then, in the orst case, be queried for each measurement for each of the P artitions, i.e. N z, P times. Note that it is dicult to give an estimate of ho many artitions are created in Distance Partitioning, because P deends on the articular measurement set that is being artitioned. Hoever, using Distance Partitioning gives at most N z, unique artitions, thus a orst case uer limit for P is N z,. The orst case comlexity of Distance Partitioning is thus aroximately ONz, 4. The orst case P = N z, gives P = N z, = N z,n z, ON 2 z,. 12 The comlexity of the em algorithm for Gaussians is given in Table 10. The comutational comlexity of one iteration of the em algorithm for Gaussian mixtures is aroximately ON x, N z, d 2 + n 3 x. On average, in our simulations and exeriments, convergence is reached in 4 iterations. Table 9: Comlexity of runing and merging Oeration Multilications Summations Comlexity ˆP s 2 d n 2 x + 5 Merge i and j? On 3 x 0 J+ 1 J m J+ n x + n x + 1 J + 1n x 2J + n x P J+ s 2 + s J + 1s 2 2J + s ν J+ + 2 J + 1 2J V J+ d 2 + d J + 1d 2 2J + d

11 Thus, the orst case comlexity of artitioning the measurement set is aroximately ON z, N x, d 2 + N 3 z, + n 3 x Overall comlexity The orst case overall comlexity of one time ste is aroximately P P O J + n 2 x + γd 2 + J + n 2 x + Nx,J 2 +n 2 3 x + N x, d 2 + N z, N x, d 2 + Nz, 4 + n 3 x 14 J+ O n 2 x + γd 2 P + N x, N z, d 2 + J+N 2 x,n 2 3 x + Nz, Inserting J + = N x, + J b and N z, = γn x, + λ into 15 gives the orst case overall comlexity O Nx, + J b n 2 x + γd 2 P + γn x, + λ N x, d 2 + N x, + J b 2 N 2 x,n 3 x + P γn x, + λ References [1] K. Granström and U. Orguner, A PHD lter for tracing multile extended targets using random matrices, IEEE Transactions on Signal Processing. [2] B.-N. Vo and W.-K. Ma, The Gaussian mixture robability hyothesis density lter, IEEE Transactions on Signal Processing, vol. 54, no. 11, , Nov [3] K. Granström, C. Lundquist, and U. Orguner, Extended Target Tracing using a Gaussian Mixture PHD lter, IEEE Transactions on Aerosace and Electronic Systems. Table 10: Comlexity of the em algorithm Oeration Multilications Summations Comlexity z j m i P i Od 3 γ i z j N x, + 3 N x, 1 2N x, + 2 m i N x, N z, + 2 N x, N z, 2N x, N z, + 2N x, P i N x, N z, d N x, 2d + d 2 N z, 1 2N x, N z, d 2 π i N x, N x, N z, 1 N x, N z, log-li On 3 x +N x, N z, d 2 + 2d + 2 9

12

13 Avdelning, Institution Division, Deartment Datum Date Division of Automatic Control Deartment of Electrical Engineering Srå Language Svensa/Sedish Engelsa/English Raortty Reort category Licentiatavhandling Examensarbete C-usats D-usats Övrig raort ISBN ISRN Serietitel och serienummer Title of series, numbering ISSN URL för eletronis version htt://.control.isy.liu.se LiTH-ISY-R-3046 Titel Title Imlementation of the GIW-PHD lter Författare Author Karl Granström, Umut Orguner Sammanfattning Abstract This reort contains seudo-code for, and a comutational comlexity analysis of, the Gaussian inverse Wishart Probability Hyothesis Density lter. Nycelord Keyords Extended target, robability hyothesis density, random matrix, Gaussian, inverse Wishart, comutational comlexity.