Instituto de Sistemas e Robótica * Pólo de Coimbra * Tracking and Data Association Using Data from a LRF

Transcription

1 Instituto de Sistemas e Robótica * Pólo de Coimbra * Tracing and Data Association Using Data from a LRF Technical Report Nº ISRLM2005/04 Cristiano Premebida October, Departamento de Engenharia Electrotécnica e de Computadores, Pólo II Universidade de Coimbra Coimbra - PORTUGAL

2 Instituto de Sistemas de Robótica Pólo de Coimbra Ano: 2005 * Laboratório de Mecatrónica * TR. Nº.: ISRLM2005/04 Tracing and Data Association Using Data from a LRF * Technical Report * Editors: Cristiano Premebida 1 Prof. Urbano Nunes Tas position: Msc. Student Scientific Advisor This wor is supported by Institute for Systems and Robotics ISR, the Portuguese Science and Technology Foundation (FCT) under grant: POSC/EEA-SRI/58279/2004, and project CyberC3: CN/ASIA-IT&C/002 (88667). 1 CPremebida is supported by the Programme Alβan, the European Union Programme of High Level Scholarships for Latin America, scholarship nº E04M029876BR Institute of Systems and Robotics ISR, Coimbra, Portugal. Local: Coimbra, Portugal Date: Authors: CPremebida

3 TRACKING AND DATA ASSOCIATION USING DATA FROM A LRF Cristiano Premebida UC/FCT/DEEC-MEEC Abstract This report presents some results and experimental evaluations on Tracing and Data Association approaches using data from a SICK/LMS200 laser sensor. Some filters are briefly described (KF, EKF, PDAF and PF) and results presented. The target (object of interest) considered in this wor is assumed to move in a 2-D space (an outdoor environment) and its motion is modeled as independent between coordinates. The motion in each coordinate has nearby constant velocity with piecewise constant white noise acceleration. Keywords Laser scanning; Kalman filters; Detection; Tracing; Data Association. Instituto de Sistemas de Robótica - ISR 3 of 17

4 Appendix 1 Introduction and Bacground Objectives Tracing of a single non-stationary target KF technique EKF Technique Standard Particle Filter situation PDAF method References Instituto de Sistemas de Robótica - ISR 4 of 17

5 1 Introduction and Bacground Some useful concepts, that will be the basis of this report, are stated bellow: Estimation: the process of inferring the value of a variable of interest from indirect, inaccurate and uncertain observations (process of measurement). Tracing: is the estimation of the state of a dynamic process (Object of interest). This is done using the tools from estimation theory and also statistical decision theory, especially when some problems (e.g. data association) are considered. Data Association: process of establishing which measurement (or weighted combination of measurement in some Validation Region) is to be used in a state estimator. Filtering: is the estimation of the current state of a dynamic process from noisy data. This report presents some results and experimental evaluations on Tracing and Data Association approaches using data from a SICK/LMS200 laser sensor. The framewor of this report, which bloc diagram is illustrated on Fig-1, was done in the context of the MTDS Project (Multi-Target Detection and Tracing System): Figure 1: Bloc Diagram of the System 2 Objectives The objectives of this text are to present some results, performed mainly in MatLab computational environment, for the tracing and data association problem relying on: - Basic tracing algorithms (KF and EKF); - Basic Particle Filter techniques; - Probabilistic Data Association Filter (PDAF). The situations of interest in this wor are: - Tracing and data association of single targets; - Tracing targets with stationary (non-maneuvering) behavior; - Tracing targets with maneuvering behavior; - Tracing of nearby targets (Cluster situation); Instituto de Sistemas de Robótica - ISR 5 of 17

6 Future wor will be focused on: - Multi-targets situation - Improvement on the techniques listed above - Cooperative sensor fusion: using the Laser and a single camera - Data association techniques in complex (realistic) situations 3 Tracing of a single non-stationary target In order to maintain a properly estimate of the process state (the tracing problem) it is crucial to carry out the data correlation process (data association) problem. Data association problem may be categorized according to what is associated what; in our case we have interest in measurement to trac association category, i.e. trac maintenance or updating. As described in [1], there are two fundamentally different approaches in associating the data: - Non-Bayesian data association: based on a decision procedure, using some statistical tools, a data association decision is made without consideration on the fact that this association is not necessarily correct. - Probabilistic (Bayesian) data association: this approach evaluates probabilities of associations and uses them throughout the estimation process. Situations of interest: I. Trac maintenance with single sensor without measurement uncertainty Non-maneuvering targets (stationary) - KF: with only uncertainty being white (Gaussian) noise with nown moments - KF: models with assumed zero-mean acceleration (jer) white noise - EKF: system/measurement are nonlinear Maneuvering targets (dynamic) - KF/EKF: with variable process noise - VSD (variable state dimension): switching from low order quiescent model to higher order maneuvering model - IE (input estimation): the unnown input (maneuver) is estimated under the assumption that it is constant over a window and the state estimate is corrected with it - MM (multiple model): II. Trac maintenance with single sensor with measurement uncertainty Single target (non-maneuvering) in clutter - Nearest (or strongest) neighbor standard KF (NNSKF): the measurement nearest to the predicted location or the strongest detection in the gate (validation region) are used as the correct one; - PDAF (Probabilistic Data Association Filter): use of all the measurements in the current time validation region, weighted by the computed association probability. Tracing with multiple sensors and/or multiples targets will not be treated in this report. Instituto de Sistemas de Robótica - ISR 6 of 17

7 The experimental results are based on the tracing of a moving object which dynamic behavior, considered here as a maneuvering model, is governed by a mobile robot (Robucar) performing a quasi circular trajectory on an outdoor environment. Incoming measurements data come from a SICK-LMS200 active laser sensor mounted in a stationary position. The variable of interest in our estimation case is the state of a dynamic system, which in our context consist of: - cinematic components: position and velocity - feature components: geometrical parameters, target classification The application of standard estimation algorithms, which would be used the measurement nearest in some sense to the predicted measurement nearest neighbor approach can lead to very poor results in an environment where spurious measurement occurs frequently. This is because such an approach does no account for the fact that measurement used in the algorithm might be originated from a source different from the target of interest. In addiction to their inaccuracy, the measurement system can be affected by additional uncertainty which is usually modeled by some additive noise. This problem (this additional uncertainty) is related to the origin of the measurement; this situation can occur in systems operating in the presence of - clutter (due to spurious reflectors or radiators near the target of interest) - decoys and countermeasures - random false alarms in the detection process - interfering targets The measurements of interest are not the raw data points from the Laser, but the outputs of the Segmentation and Feature Extraction blocs. In the sequel, we will present some results for the tracing of a single object (target). The modeling process will be carried out using two approaches: - Standard linear model - Non-linear standard model - Expanded model: acceleration component is added as a state to be estimated System model Considering the discrete-time controlled process that is governed by the linear stochastic difference equation x ω, = Ax 1 + Bu + 1 x n R (1) with measurements modeled by z = + ν, Hx z m R (2) Instituto de Sistemas de Robótica - ISR 7 of 17

8 The random variables ω and ω represent the process and measurement noise (respectively), and are assumed to be zero-mean, white and independent, with normal probability distributions: p( ω) N(0, Q) p( ν ) N(0, R) (3) The symbol N(x,µ,S) stands for the normal (Gaussian) pdf with arguments: the (vector) random variable x, mean µ, and covariance S. In this wor, the motion of a target (object of interest) is best described in Cartesian coordinates, with the state-space vector: x p p = v v x / y / x / y / (4) where: - p x : geometrical center position of the object related to X-axis - p y : geometrical center position of the object related to Y-axis - v x : linear velocity in direction of the X axis - v y : linear velocity in direction of the Y axis Target model The target is assumed to move in a 2-Dimensional space and its motion is modeled as independent between coordinates. The motion in each coordinate has nearby constant velocity with piecewise constant white noise acceleration. The measurements are position only (range,bearing), with additive white noise with variance (σ r ) in each coordinate. Variables on consideration: Polar coordinates True range/bearing: Measured range/bearing: Error*: *_ assumed independent, with zero-mean and standard deviation σ r, σ θ. Cartesian coordinates True range/bearing: Measured range/bearing: Error: - exact error: - linearized error: Without necessity of demonstration, the straightforward equations define the variables stated before: Instituto de Sistemas de Robótica - ISR 8 of 17

9 The following covariance matrices will be used on simulations: r + mσθ sin θm σ r cos θm ( σ r rmσ θ )sinθm cosθm RKF = ( σ r rmσ θ )sinθm cosθm rmσ θ cos θm + σ r sin θm (5) 2 σ 0 R = r EKF 2 0 σ θ (6) 3.1 KF technique The well nown fundamental assumptions of the KF are: 1- Evolution of the system s state according to a nown linear process equation driven by: - A nown input - An additive process noise, zero-mean white (uncorrelated) process with nown covariance Q(). 2- Measurements - a nown linear function of the state with - An additive measurement noise, zero-mean white with nown covariance R() 3- Initial state unnown, assumed to be a random variable with a nown mean (initial) and covariance (initial uncertainty) 4- Initial error and noises, mutually uncorrelated As almost all recursive algorithms, the state estimation cycle consists of I- Prediction: of the state and measurements (time update) II- Update: of the state (measurement update) Remars Under the Gaussian assumption for the initial state error and all the noises entering into the system, the KF is the MMSE state estimator. If the above random variables are not Gaussian, and one has only their first two moments, then the KF is the best linear MMSE estimator (in the context/class of linear estimator). The covariance calculations are independent of the state, and can be performed offline. Limitations on using KF - nonlinear motion models - nonlinear measurement equations - unnown inputs into the dynamic equation and/or mode changes - auto or cross-correlated noises - uncertain origin measurements (data association uncertainty) - unnown number of multiple-targets Considering the inputs equal zero, i.e. u =0, the state prediction equations on X- direction are given by: p v x / x / 1 = 0 t p 1 v x / 1 x / 1 ω px + ωvx / 1 / 1 (7) with the measurement Instituto de Sistemas de Robótica - ISR 9 of 17

10 px / = [ 1 ] + vx (8) z px / 0 / v x / In a similar manner, the Y-direction equations: p v y / y / 1 = 0 t p 1 v y / 1 y / 1 ω py + ωvy / 1 / 1 (9) where: py / z py / = [ 1 0] + vy / v (10) y / Results The object of interest to be traced is assumed having a constant linear velocity motion, executing a semi-circular trajectory in an outdoor environment. The measurements are done in polar coordinates (r,θ) using the Laser sensor and we have interest in tracing the Cartesian form, thus r cos( θ ) z polar = ( r, θ ) zcartesinan = (11) r sin( θ ) This transformation avoids the use of a nonlinear function model. Fig-2 presents the practical experiment on estimating/updating the Cartesian position of the Object over time KF - Tracing the position of a moving object Estimated Measurement Position - Y Position - X Figure 2: Tracing the position of the target using a KF Instituto de Sistemas de Robótica - ISR 10 of 17

11 3.2 EKF Technique Modeling assumptions (the same as for the KF) - The dynamic and/or the measurement equation are nonlinear functions Based on first or second order Taylor expansion series of the nonlinear functions (in the prediction stage), we have the 1 st or 2 nd order EKF. The use of the series expansion in the state/measurement prediction has the undesired potential of introducing unmodeled errors that violate some basic assumptions about the prediction errors. In general, a nonlinear transformation will - introduce a bias (in contrast with the zero-mean consideration), and - the covariance calculation based on a series expansion is not always accurate. The measurements are done in polar coordinates, i.e. the nonlinearities are postpone to the Filter, thus z ] T = [ r, θ (12) These expansions rely on Jacobian/Hessians evaluated at the estimated state (rather then the exact state) and neglect the higher order terms. By the way, the EKF are very sensitive to the accuracy of the initial conditions. Nevertheless there are some techniques to compensate these undesired situations, for example: using an IEKF, using artificial noise, etc [1]. In this case, we have the same equations, as the KF case, for the state prediction but, the measurement prediction is the nonlinear mapping: z = h( x, ν, ) (13) where 2 2 1/ 2 ( p + x py ) h = (14) arctan( py / px) As stated before, our state-space vector is represented by x p p = v v x / y / x / y / Results The nonlinearities are treated in a suboptimal way using the Jacobian (1 st order EKF) of the function h evaluated at the predicted state estimate, thus Instituto de Sistemas de Robótica - ISR 11 of 17

12 h1( + 1) h1 ( + 1) K x1 x2 h2 ( + 1) h2 ( + 1) H = h = K x1 x (15) 2 M O M x = xˆ ( ) In our case, the measurement covariance update is 2 2 1/ / 2 x1 ( x1 + x3 ) 0 x3( x1 + x3 ) 0 H = (16) 2 x3x1 [(1 + ( x3 / x1 ) ] 0 [ x1 (1 + ( x3 / x1 ) ] 0 Fig-3 presents the practical experiment on estimating/updating the Cartesian position of the Object over time Tracing using an EKF Estimated Measurement Y - Position X - Position Figure 3: Tracing the position of the target using a 1 st order EKF 3.3 Standard Particle Filter situation PF is an available method for tracing a variable of interest with (typically) a non- Gaussian and (potentially) multi-modal pdf [3]. The basis of this method is to use a sample-based construction to represent the entire pdf. Multiple samples (copies) of the variable of interest, i.e. the state of the process, are used in the PF, each one with the associated weight that signifies the quality of that sample; these are nown as particles. Instituto de Sistemas de Robótica - ISR 12 of 17

13 A formal (general) description of the PF algorithm is presented above Require: A set of particles at time 0: S 0 i =[x j,w j :j=1 M] W = w j :j=1 M {number of particles} while (Exploring) do = +1; if (ESS(W)<B*M) then {Particle population depleted} Index = Resample(W); S i = S i (index); end if for (j=1 to M) do {Prediction} x +1 j = f(x j,); end for S = Sensing(); for (j=1 to M) do {Update the weights} w +1 j = w j *W(s, x +1 j ); end for for (j=1 to M) do {Normalize the weights} w +1 j = w j /Σ w j end for end while Fig-4 presents the practical experiment on estimating/updating the Cartesian position of the Object over time. Figure 4: Tracing the position of the target using a Particle Filter Instituto de Sistemas de Robótica - ISR 13 of 17

14 3.4 PDAF method The Probabilistic Data Association (PDA) algorithm calculates the association probabilities for each validated measurement at the current time to the target of interest. This probabilistic (Bayesian) information is used in the tracing filter called PDA filter (PDAF), which accounts for the measurement origin uncertainty. This filter is particularly useful in the situation where a tracing filter receives more then one validated measurements, i.e. the origin uncertainty case: - From which target, if any, a particular measurement originated? In particular, the problem of extraneous measurements (due to random phenomena: false alarms or clutter) as well as neighbor targets. A measurement in the gate (i.e. a validated measurement), while not guaranteed to have originated from the target the gate pertains to, is a valid association candidate thus the name validation region or association region. If there is more then one detection (measurement) in the gate, this leads to an association uncertainty. The PDAF [2] algorithm is quite similar as in the KF based one. The Prediction (cycle) of the state and measurement is PREDICTION xˆ( + 1 ) = F( ) xˆ( ) zˆ( + 1 ) = H ( + 1) xˆ( + 1 ) (17) with covariance of the predicted state P ( + 1 ) = F( ) P( ) F( )' + Q( ) (18) Innovation covariance corresponding to the true measurement S ( + 1) = H ( + 1) P( + 1 ) H ( + 1)' + R( + 1) (19) MEASUREMENT SELECTION FOR UPDATE The validation region (gate/ellipsoid)* 1 υ( + 1) = { z :[ z zˆ( + 1 )]' S( + 1) [ z zˆ( + 1 )] λ} (20) where λ is the gate threshold determined by the chosen gate probability P G. *_ The ellipse (or ellipsoid) of probability concentration - the semi axis of the ellipse are the square roots of the eigenvalues of λs, where λ is the gate threshold and S is the covariance matrix of the innovation. Validated measurements { z i ( + 1), i = 1,..., m( + 1)} (21) Innovation corresponding to the i-th validated measurement υ ( + 1) = z ( + 1) zˆ( + 1 ) i = 1,..., m( + 1) (22) i i Instituto de Sistemas de Robótica - ISR 14 of 17

15 PROBABILISTIC DATA ASSOCIATION NONPARAMETRIC VERSION Probability that the i-th validated measurement is the correct one ei m( b + j βi( + 1) = b m( b + j + 1) = 1 + 1) = 1 e e j j i = 1,..., m( + 1) i = 0 (23) Where β 0 (+1) is the probability that none of the measurement is correct, and ei e n π b λ 1 1 i υi ( + 1)' S ( + 1) 2 2 z / 2 m( + 1) c 1 n υ ( + 1) z 1 PD P P D G (24) UPDATE State update xˆ ( ) = xˆ( + 1 ) + W ( + 1) ν ( + 1) (25) with the combined innovation m( + 1) i i= 1 ν ( + 1) β ( + 1) ν ( + 1) (26) Covariance associated with the updated state i ~ P( ) = P( + 1 ) [1 β 0 ( + 1)] W ( + 1) S( + 1) W ( + 1)' + P( + 1) (27) where the (weighted) spread of the innovation term is m( ~ ( + 1) ( + 1)[ + P W i= 1 1) β ( + 1) ν ( + 1) ν ( + 1)' ν ( + 1) ν ( + 1)'] W ( + 1)' (28) i i i A commented version of this algorithm is available on section (Annex) 5.2. Fig-5 presents the practical experiment on estimating/updating the Cartesian position of the Object over time. Instituto de Sistemas de Robótica - ISR 15 of 17

16 1200 Tracing of a 2D motion using the PDAF Estimated Detection Y-axis Position X-axis Position Figure 5: Tracing the position of the target using the PDAF Instituto de Sistemas de Robótica - ISR 16 of 17

17 4 References [1] Y. Bar-Shalom and T.E. Fortmann (1988). Tracing and Data Association. Academic Press. [2] Y. Bar-Shalom and X.R. Li (1995). Multitarget-Multisensor Tracing: Principles & Techniques. YBS Publishing. [3] Ioannis Releitis (2004). A Particle Filter Tutorial for Mobile Robot Localization. Technical Report TR-CIM-04-02, Centre for Intelligent Machines, McGill University, Montreal, Quebec, Canada. Instituto de Sistemas de Robótica - ISR 17 of 17