The Kernel-SME Filter with False and Missing Measurements

Transcription

1 The Kernel-SME Filter with False and Missing Measurements Marcus Baum, Shishan Yang Institute of Computer Science University of Göttingen, Germany marcusbaum, Uwe D Hanebec Intelligent Sensor-Actuator-Systems Laboratory (ISAS Institute for Anthropomatics and Robotics Karlsruhe Institute of Technology (KIT, Germany uwehanebec@ieeeorg Abstract The recently proposed Kernel-SME filter for multiobject tracing is a further development of the Symmetric Measurement Equation (SME idea introduced by Kamen in the 199s The Kernel-SME constructs a symmetric, ie, permutation invariant, measurement equation by transforming the measurements to a ernel mixture function This transformation is scalable to a large number of objects and allows for deriving an efficient closed-form Gaussian filter based on the Kalman filter formulas This wor shows how the Kernel-SME approach can systematically incorporate false and missing measurements Keywords Multi-Object Tracing, Data Association, Symmetric Measurement Equation I INTRODUCTION Multi-object tracing deals with the problem of successively estimating the number and states of multiple objects based on noisy unlabeled measurements [3, [15 One of the main challenges in multi-object tracing is that the measurements are unlabeled, ie, it is unnown which measurement belongs to which object As the number of feasible association hypotheses explodes with the number of objects and measurements, sophisticated approximations are required in order to obtain efficient algorithms In this context, many different multi-object tracing algorithms have been developed, eg, the Joint Probabilistic Data Association Filter (JPDAF [1 computes a Gaussian approximation of the posterior density of the objects states and the Probability Hypothesis Density (PHD filter [14, [18 is based on Finite Set Statistics (FISST The Kernel-SME [4, [1 filter discussed in this paper is based on the ey idea of the Symmetric Measurement Equation (SME filter [8, [9, [12, [13, [17, [19, [2 The SME filter avoids an explicit enumeration of data association hypotheses by constructing a pseudo-measurement with a symmetric transformation A symmetric transformation is permutation invariant to the order of its arguments, so that the correct permutation, ie, association, is not required We argue that the original SME approach comes with three main problems: (i existing SMEs suffer from strong nonlinearities, (ii do not scale well with the number of measurements, and (iii cannot systematically deal with false and missing measurements We note that [11 introduces an approach for incorporating false and missing measurements in the original SME approach based on explicit enumeration of association hypotheses In our previous wor about the Kernel-SME filter [4, [5, we addressed problem (i and (ii by proposing a novel SME that maps the measurements to a ernel mixture function In this manner, a data-dependent symmetric transformation is constructed that scales well with the number of measurements In this wor, we address problem (iii: We show that clutter and missed detections can be systematically incorporated in the Kernel-SME framewor, ie, modeled within the symmetric transformation without the enumeration of association hypotheses In the following section, we describe the considered multiobject tracing problem Section III introduces the novel symmetric measurement equation for false and missing measurements Based on this measurement equation, we subsequently derive a Gaussian state estimator using the Kalman filter formulas in Section IV After presenting numerical simulations that demonstrate the performance of the Kernel-SME filter in Section V, the paper is concluded in Section VI II PROBLEM FORMULATION We treat the tracing of multiple objects based on noisy unlabeled measurements, ie, the measurement-to-object association is unnown Furthermore, we impose the following assumptions: A1 A2 A3 The number of objects N is nown Each object gives rise to at most one measurement per scan/frame The detection probability is denoted as p d The number of false measurements is Poisson distributed with mean λ The d x -dimensional object state vectors are denoted as x 1,, xn, where denotes the discrete time index1 The single object states are staced in a joint state vector x [ (x 1 T,, (x N T T IR d x N A Measurement Model At each time, a set of M measurements Y y 1,, ym IRdy 1 Vectors are underlined, eg, x, and random variables are printed in bold, eg, x and x

2 is available The measurement set is related to the states according to Y C Θ l (1 where 1 l N C consists of M c clutter measurements that are uniformly distributed in the tracing region The number of clutter measurements M c is Poisson distributed with mean λ Θ l consists of the measurement from object l (if detected, ie, H Θ l i x l + vl if detected, (2 otherwise where H l is the measurement matrix and vl zeromean noise with covariance matrix Σ v,l B System Model The motion model of an individual object is specified by x l +1 A l x l + w l, (3 where A l is the system matrix and wl is additive white noise with covariance matrix Σ w l All individual motion models (3 can be staced into a single vector x 1 +1 x N +1 x +1 A 1 :A III x 1 + A N x N x KERNEL-SME w 1 w N w (4 The measurement equation (1 relates the object states with the measurements However, it relates two sets and it is not clear which element on the left-hand side is associated to which element on right-hand side, ie, the association is not nown The basic idea of the Kernel-SME approach is to convert the set-valued equation (1 to an equivalent vector-valued equation using a symmetric transformation of the measurements This transformation is composed of sums of ernels according to S Y (z y Y N (z y; Γ, (5 where Γ is a suitable ernel width and z IR dy is a free parameter Intuitively, (5 interprets the measurements as a Gaussian mixture function, where the means coincide with the measurements Also note that the order of the measurements in Y does not affect the result of S Y (z in (5; it is a symmetric transformation, ie, a set function The application of (5 to (1 yields to S Y (z S C (z + S 1 l N Θl (z (6 S C (z + S Θ i (z (7 1 i N S C (z + 1 i N d i S H i x i +vi (z, (8 where d i, 1 indicates if the object is detected, ie, p(d i 1 p d and p(d i 1 p d The transformed measurement in (6 is scalar and still involves the free parameter z In order to get a sufficient number of measurement equations, we suggest to instantiate z with different test vectors a 1,, ana, which yields S Y (a 1 S C (a 1 d i S H i 1 i N xi +vi (a 1 a + S Y (a Na S C (a Na d i S H i 1 i N xi +vi (a Na a s c (9 Intuitively, the test vectors are supposed to lie close to the predicted target locations in order to capture the shape of the transformed measurements IV GAUSSIAN ESTIMATOR FOR THE KERNEL-SME In the same manner as in [4, we derive a (nonlinear Kalman filter for recursive estimation based on the measurement equation (9 For this purpose, we assume that the posterior probability density function of x is Gaussian, ie, ( p(x S N x ; µ x, Σx, (1 where µ x is the mean, Σx is the covariance matrix, and S s 1,, s all pseudo-measurements up to time 1 Time Update: ( The time update step determines p(x S 1 N x ; µ x, 1 Σx 1 based on the previous density p(x 1 S 1 Due to the linear system model, the prediction can be performed according the Kalman filter formulas µ x A 1 µ x, and (11 1 Σ x 1 A Σ x 1(A T + Σ w, (12 where Σ w denotes the covariance matrix of the staced system noise w 2 Measurement ( Update: In the measurement update step, the prediction N x ; µ x, 1 Σx 1 is updated with the pseudo-measurement s For this purpose, we apply the Kalman filter formulas [2 to the measurement equation (9, ie, the updated estimate becomes µ x ( µ x + 1 Σxs (Σ ss 1 s µ s, and (13 Σ x Σ x 1 Σxs (Σ ss 1 Σ sx, (14 with predicted pseudo-measurement µ s, cross-covariance Σ xs between the state x and pseudo-measurement s, and covariance Σ ss of the pseudo-measurement s Closed-form expressions for the above moments can be derived similar to the case without missing and false measurements, see [4 The detection indicator d can be incorporated in

3 the moment calculations in a straightforward manner as it is mutually independent to all other involved random variables Furthermore, the clutter measurements are independent of object-related measurements, ie, c is independent to the transformed object-related measurements The following theorem derives the moments for the transformed clutter measurements Theorem 1 Let C y 1,, c ym in (9 be a set of M c independent identically distributed random variables, where M c is Poisson distributed with mean λ > Then, the mean and covariance of the transformed clutter c in (9 is given by µ ci λ E S y (z Σ cicj λ CovS y (a i, S y (a j + λ E S y (a i E S y (a j with i, j 1,, N a, where y has the same distribution as the random variables of C Proof: Let M c Bin(n, p be Binomial distributed with n trials of probability p It is well nown that the Binomial distribution converges to a Poisson distribution with λ np for n and p Hence, we obtain for the mean and for the covariance n ES C (z E d i (z np E S y (z λ E S y (z CovS C (z 1, S C (z 2 Covd i (z 1, d i (z 2 E p d E d 2 i (z 1 (z 2 E d i (z 1 (z 2 p 2 d E (z 1 E (z 1 E np d E S y (z 1 S y (z 2 np 2 d E S y (z 1 λ E S y (z 1 S y (z 2 λ CovS y (z 1, S y (z 2 + λ E S y (z 1 d i (z 2 (z 2 E S y (z 2 E S y (z 2 Fig 1 depicts pseudo-code of the overall algorithm for the measurement update under the assumption that the object estimates are mutually uncorrelated, which is a standard assumptions in multi-object tracing, ie, Σ xixj for all i j However, we note that this assumption is actually not necessary for the Kernel-SME filter; it just leads to simpler formulas, see also [4 The overall run-time of the algorithm is O((N a2 N + M, which is pretty efficient compared to other multi-object tracers With further approximations, even more efficient algorithms could be achieved, eg, assuming Σ ss to be diagonal would lead to a linear-time algorithm in the number test-points and objects For each predicted object location, the pseudo-code in Fig 1 selects two test vectors for each axis In this manner, the number of test vectors does not depend on the number of measurements More advanced methods for selecting test vectors might be reasonable In general, an increasing number of test vectors leads to an increased performance of the Kernel- SME filter V EVALUATION In order to assess the performance of the Kernel-SME filter, we consider a scenario with six two-dimensional target objects The objects move according to the trajectory depicted in Fig 2a For the tracer, a nearly constant velocity model is employed, ie, the state vector is [ x l (p l T, (ṗ l T T, with l 1,, N, where p l R2 is the position and ṗ l R2 the velocity Furthermore, for the process model (3, we have [ A l I2 I 2 2 I 2 with two-dimensional identity matrix I 2 and null matrix 2 The process noise covariance matrix is [ I24 Σ w I 22,l q I 22 I 2 with parameter q 4 At each time, noisy position measurements are received, ie, H l [I 2, 2, with Σ v,l 2I 2 The probability of detection is 95 and the number of false measurement is Poisson distributed with mean λ 7, where the false measurements are uniformly distributed in the tracing area Fig 2b shows the overall measurements for an example run of 5 time steps The Kernel-SME filter uses the ernel width Γ 1 The Kernel-SME filter falls into the category of computationally cheap and simple multi-object tracing algorithms Hence, we compare the results with the Cheap JPDAF [6, which is a traditional fast approximation of the weights in the JPDAF filter Both tracers are initialized close to the ground truth For the comparison, we show the average OSPA error [16 for 5 Monte Carlo runs VI CONCLUSIONS AND FUTURE WORK This paper is about the Kernel-SME filter [4 a new method for multi-object tracing algorithm based on Kamen s Symmetric Measurement Equation (SME idea The Kernel- SME employs a fundamental new construction of SMEs that aims at solving the typical problems associated to the traditional SME approach Specifically, in this paper we show how false and missing measurements can be incorporated in the Kernel-SME filter This is a significant step forward as nearly all real-world applications come with false and missing measurements

4 Input: Predicted states µ x1,, 1 µx N of N objects with covariance matrices Σx1 1 1,, Σx N 1 Measurements y 1,, ym (order of measurements is irrelevant Output: Updated state estimates µ x1,, µx N Algorithm: 1 Determine test vectors a 1,, ana a l+i 1 of N objects with covariance matrices Σ x1,, Σx N with N a 2 d y N according to and : H l µ x l 1 + ( dy Γ i a l+2(i 1 : H l µ x l 1 ( dy Γ for i 1,, N and l 1,, d y, where ( d y Γ i denotes the i-th column of d y Γ [ T 2 Compute pseudo-measurement s s 1,, sna with N s i N (a i ; yl, Γ for i 1,, N a ( 3 Determine Pl Γ(z : N z; H l µx, l Hl Σx l 1 (Hl T + Σ v + Γ for all l 1 N 4 Determine moments of pseudo-measurements: T a Mean µ [µ s s1,, µs Na of predicted pseudo-measurement: µ s p,i d Pl Γ (a i + for i 1,, N µci a b Covariance Σ ss Σ sisj l1 l1n (Σsisj i,j1,,na of predicted pseudo-measurement: p d N (a i ; aj, 2Γ l1n Pl 5Γ ( 1 2 (ai + aj p2 dpl Γ (a i P l Γ (a j + Σcicj 5 Perform update for all objects l 1 N: a Cross-covariance Σ x ls [ Σ x ls 1,, Σ x ls Na between predicted pseudo-measurement: b Kalman filter update Σ x ls i µ x l µsi K l Σ x l 1 Hl µ x l + p d Pl Γ (a i (µx l + 1 Kl (a i Hl µ x l, where ( 1 H l Σ x l 1 (Hl T + Γ + Σ v µ x l Σ x l Σ x l + 1 Σx ls (Σss 1 Σx ls (Σss 1 ( s µ s 1 Σ sx l, and Fig 1: Summary of the measurement update of the Kernel-SME filter i The Kernel-SME filter is very efficient and involves rather simple formulas Experiments show that the tracing quality can compete with traditional fast and simple multi-object tracing algorithms Of course, the evaluation of multi-object tracing algorithms is in general quite complex and many different aspects have to be considered Future evaluations and experiments will bring up the advantages and disadvantages of the Kernel-SME filter Of course, due to its low computational demands, one cannot expect too much with regards to tracing quality Obviously, the number and locations of the test vectors influences the quality of the Kernel-SME filter Hence, future wor shall analyze and discuss different methods for selecting test vectors We also intend to extend the Kernel-SME filter to the case of an unnown number of objects and to the case of multiple measurements per object per time frame Furthermore, Kernel-SME ideas have inspired new association-free multiobject tracing methods that are inherently association-free such as [7 that will be further investigated in the future REFERENCES [1 Y Bar-Shalom, F Daum, and J Huang, The Probabilistic Data Association Filter, IEEE Control Systems Magazine, vol 29, no 6, pp 82 1, December 29 [2 Y Bar-Shalom, T Kirubarajan, and X-R Li, Estimation with Applications to Tracing and Navigation New Yor, NY, USA: John Wiley & Sons, Inc, 22

5 y y Mean OSPA Error K-SME Cheap x (a Initial object locations (big dots and trajectory (small dots x (b Measurements from an example run Fig 2: Evaluation: Kernel-SME vs Cheap JPDAF time step (c Mean OSPA error (for 5 runs [3 Y Bar-Shalom, P K Willett, and X Tian, Tracing and Data Fusion: A Handboo of Algorithms YBS Publishing, 211 [4 M Baum and U D Hanebec, The Kernel-SME Filter for Multiple Target Tracing, in Proceedings of the 16th International Conference on Information Fusion (Fusion 213, Istanbul, Turey, Jul 213 [5 M Baum, B Noac, and U D Hanebec, Kalman Filter-based SLAM with Unnown Data Association using Symmetric Measurement Equations, in Proceedings of the 215 IEEE International Conference on Multisensor Fusion and Information Integration (MFI 215, San Diego, California, USA, Sep 215 [6 R J Fitzgerald, Development of Practical PDA Logic for Multitarget Tracing by Microprocessor, in American Control Conference (ACC, 1986, pp [7 U D Hanebec and M Baum, Association-Free Direct Filtering of Multi-Target Random Finite Sets with Set Distance Measures, in Proceedings of the 18th International Conference on Information Fusion (Fusion 215, Washington, USA, Jul 215 [8 E Kamen, Multiple Target Tracing based on Symmetric Measurement Equations, IEEE Transactions on Automatic Control, vol 37, no 3, pp , March 1992 [9 E Kamen and C Sastry, Multiple Target Tracing using Products of Position Measurements, IEEE Transactions on Aerospace and Electronic Systems, vol 29, no 2, pp , April 1993 [1 G Kurz, M Baum, and U D Hanebec, Real-time Kernel-based Multiple Target Tracing for Robotic Beating Heart Surgery, in Proceedings of the Eighth IEEE Sensor Array and Multichannel Signal Processing Worshop (SAM 214, A Coruña, Spain, Jun 214 [11 Y J Lee and E W Kamen, SME Filter Approach to Multiple Target Tracing with False and Missing Measurements, in SPIE 1954, Signal and Data Processing of Small Targets, 1993, pp [12 W Leven and A Lanterman, Multiple Target Tracing with Symmetric Measurement Equations using Unscented Kalman and Particle Filters, in Proceedings of the Thirty-Sixth Southeastern Symposium on System Theory, 24, pp [13, Unscented Kalman Filters for Multiple Target Tracing With Symmetric Measurement Equations, IEEE Transactions on Automatic Control, vol 54, no 2, pp , February 29 [14 R P S Mahler, Statistical Multisource-Multitarget Information Fusion Boston, Mass: Artech House, 27 [15 B Ristic, S Arulampalam, and N Gordon, Beyond the Kalman Filter: Particle Filters for Tracing Applications Artech House Publishers, 24 [16 D Schuhmacher, B-T Vo, and B-N Vo, A Consistent Metric for Performance Evaluation of Multi-Object Filters, IEEE Transactions on Signal Processing, vol 56, no 8, pp , Aug 28 [17 Swati and S Bhaumi, Multiple Target Tracing based on Homogeneous Symmetric Transformation of Measurements, Aerospace Science and Technology, 212 [18 B-N Vo and W-K Ma, The Gaussian Mixture Probability Hypothesis Density Filter, IEEE Transactions on Signal Processing, vol 54, no 11, pp , Nov 26 [19 F Zhang, G Hinz, D Clare, and A Knoll, Vehicle-infrastructure localization based on the SME filter, in IEEE 18th International Conference on Intelligent Transportation Systems (ITSC IEEE, 215, pp [2 F Zhang, G Hinz, D Gulati, D Clare, and A Knoll, Cooperative vehicle-infrastructure localization based on the symmetric measurement equation filter, GeoInformatica, pp 1 2, 216