Implementation of Particle Filter-based Target Tracking

Transcription

1 of -based V. Rajbabu VLSI Group Seminar Dept. of Electrical Engineering IIT Bombay November 15, 2007

2 Outline Introduction 1 Introduction / 55

3 Outline Introduction 1 Introduction / 55

4 Introduction Introduction Problem scenario Multi-target tracking - batch measurements [Volkan Cevher] Particle filters - computational complexity Sensors operating under constrained environment Problem Illustration Objective Efficient digital implementation of particle filters Real-time Low-power 4 / 55

5 Outline Introduction 1 Introduction / 55

6 Tracking Introduction What is tracking? Tracking - process measurements to sequentially estimate hidden states Target tracking Visual tracking Applications Surveillance and monitoring Medical imaging Robotics Motion in sports 6 / 55

7 Introduction We consider multi-target tracking State vector - target or vehicle kinematic characteristics, ex., 2 D position and velocity, or motion parameters in polar coordinates Measurements - range or angle with respect to sensor 7 / 55

8 Introduction State-space Formulation Dynamic state-space problem - sequential State-space model depends on physics of the problem System transition equation x t = f t (x t 1, u t ), u t system noise f t () system evolution function (possibly nonlinear) Observation equation y t = g t (x t, v t ), v t measurement noise g t () measurement function (possibly nonlinear) 8 / 55

9 Introduction Bearings-only Tracking Automated estimation of a moving target s state using angle measurements (bearings) State update equation X t = FX t 1 + Gu t, where X t = [x v x y v y ] T t, u t = [u x u y ] T t, F = and G = Measurement equation z t = arctan {y t /x t } + r t 9 / 55

10 Introduction Batch-based Tracking State Vector Multiple target state vector - independent partitions Constant velocity during batch period T X t = [x T 1 (t), xt 2 (t),, xt k (t)]t θ k (t) x k (t) = R k (t) v k (t), k target index φ k (t) θ k (t) direction-of-arrival (DOA) R k (t) log range (range) v k (t) velocity φ k (t) heading direction 1 φ(t) θ(t) r(t) Y vt r(t+t) 2 θ(t+t) X 10 / 55

11 Introduction Batch-based Tracking Observation Model Image template-matching - batch measurements DOA measurements from acoustic sensor - beamforming Range measurements from radar sensor Observability - batch of minimum three measurements DOA or Range Missing data DOA or Range hmτ θ,r (x(i) ) h θ,r mτ (x(j) ) Batch Clutter t 0 Mτ T 2T 3T y t = {y t+mτ (p)} M 1 m=0 t+τ t+t t t+(m 1)τ Figure: Template-matching 11 / 55

12 State-Space Model Batch-based range tracking Non-linear state transition equation θ k (t+t) R k (t+t) v k (t+t) φ k (t+t) = j tan 1 e R ff k sin θ k +Tv k sin φ k e R k cos θ k +Tv k cos φ k 1 2 log{e2r k +T 2 vk 2+2TeR k v k cos(θ k φ k )} v k φ k where u k (t) Gaussian process noise + u k(t) Observation likelihood ( p ( y(t) x k (t) ) M 1 ff Q 1+ 1 κ P exp j ) (hr mτ (x k (t)) y t+mτ (p i ))2 m=0 2πσr 2κλ p 2σ 2 i r

13 Outline Introduction 1 Introduction / 55

14 Introduction Objective - estimate hidden state x t from observations y t Probabilistic description - using pdf s prior distribution - p(x 0 ) state transition - p(x t x t 1 ) data likelihood - p(y t x t ) Bayesian estimation - minimum mean square estimate Conditional mean E(x t y t ) of the posterior distribution p(x t y t ) p(y t x t ) p(x t x t 1 ) 14 / 55

15 Introduction Sequential Representation Cumulative state up to time t: X t = {x j, j = 0,..., t} Cumulative measurement up to time t: Y t = {y j, j = 0,..., t} Bayesian estimation Estimate x t based on all available measurements up to t by constructing the posterior p(x t Y t ) 15 / 55

16 Recursive Filter Introduction Consists of two steps Prediction step: p(x t 1 Y t 1 ) p(x t Y t 1 ) Update step: p(x t Y t 1 ), y t p(x t Y t ) Given the pdf s Prediction step: p(x t Y t 1 ) = p(x t x t 1 )p(x t 1 Y t 1 )dx t 1 Update step: p(x t Y t ) = p(y t x t )p(x t Y t 1 ) p(y t Y t 1 ) where, p(y t Y t 1 ) = p(y t x t )p(x t Y t 1 ) 16 / 55

17 Recursive Filter Introduction Recursive application of prediction and update steps provides the optimal Bayesian solution Minimum mean square estimate (MMSE) is the conditional mean - E(x t Y t ) Analytically intractable - integrals and pdf s 17 / 55

18 Kalman Filter Introduction Optimal solution for the recursive problem exists Kalman filter - optimal solution if state and measurement models - linear, and state and measurement noises - Gaussian Extended Kalman filter (EKF) - extension of Kalman filter state and/or measurement models - nonlinear, and state and measurement noises - Gaussian 18 / 55

19 Outline Introduction 1 Introduction / 55

20 Introduction Suboptimal filter - for nonlinear systems and non-gaussian noise handles nonlinearity as such - without linearisation handles multimodal distributions Based on Monte Carlo methods Monte Carlo - randomly chosen Other names Sequential Monte Carlo (SMC) methods Bootstrap filter Sequential Importance Sampling (SIS) filter CONDENSATION - CONditional DENSity propogation 20 / 55

21 Introduction Monte Carlo Integration To numerically evaluate: I = q(x)dx where x R nx Monte Carlo (MC) method Factorize: q(x) = f (x) π(x), s.t., π(x) is a pdf. We can draw N samples {x i ; i = 1,...,N} MC estimate of I = f (x)π(x)dx is the sample mean I N = 1 N N f (x i ) i=1 What if it is difficult to draw samples from π(x)? 21 / 55

22 Introduction Importance Sampling Choose an Importance distribution g(x), such that: π(x) > 0 g(x) > 0 for all x R nx. and is easy to draw samples from g(). MC estimate - generate samples {x (i) } g(x) I N = 1 N N f (x (i) ) w(x (i) ) i=1 where, w(x (i) ) = π(x(i) ) g(x (i) ). 22 / 55

23 Introduction Importance Sampling Target distribution π(x) Proposal distribution g(x) Random Support N = 20 x Unnormalized weights : w j u = Target distribution s expectation: E π {F(x)} x (j) N(µ,σ 2 ) 1 exp (2πσ 2 ) π(x j (j) ) 20 j=1 F(x(j) )w j u 20 j=1 w(j) u (x(j) µ) 2 2σ 2 ff 23 / 55

24 Introduction - (1/2) Use randomly chosen particles to represent posterior distribution { } x (i) t, w (i) N t, i=1 x (i) t w (i) t : support points : associated weights N number of particles Discrete weighted approximation to the posterior p(x t Y t ) N i=1 w (i) t δ(x X (i) t ) 24 / 55

25 Introduction - (2/2) Discrete approximation of distribution using Importance Sampling Particles - determine the support region Weights - proportional to probabilities Proposal or importance function plays a critical part Recursive update by propagating particles and weights Use updated distributions to obtain estimates 25 / 55

26 Resampling Introduction Degeneracy of particles - after a few iterations most particles have negligible weights Resampling Eliminate or replicate particles depending on their importance weights Image adapted from Dr.Volkan Cevher Weights Do estimation Resampling 26 / 55

27 Introduction Sequential Importance Resampling (SIR) Given the observed data y k at k, do 1. For i = 1, 2,..., N, Sample particles: x (i) k g(x k x (i) k 1, y k). 2. For i = 1, 2,..., N, Calculate the importance weights: w (i) k For i = 1, 2,..., N, Normalize the weights: w (i) k = w(i) k P N j=1 w(i) k = p(y k x (i) k )p(x(i) k x(i) k 1 ) g(x (i) k x(i) k 1,y k) 3. Calculate the state estimates: E{f (x t )} = N { } t 4. Resample { x (i) k, w(i) k } to obtain new set of particles. x (j) k, w(j) k = 1 N. i=1 w(i). f (x (i) t ). 27 / 55

28 Introduction PF Flow - Suboptimal Proposal Function w (i) t =w (i) t T p(yt X(i) t ) Initialization Propose Particles Evaluate Weights Normalize Weights Resample Particles Measurements Estimate States NP i=1 w (i) t X (i) t Proposal function - state update Output 28 / 55

29 Introduction PF Flow - Optimal Proposal Function Initialization Propose Particles w (i) t =w (i) t T Evaluate Weights p(yt x (i) t )p(x (i) t x t T ) Qk g k (x(i) k (t) y t,x (i) k (t T)) Normalize Weights Resample Particles Measurements Estimate States NP i=1 w (i) t X (i) t Proposal function - full-posterior Output 29 / 55

30 Introduction Example: 1D Estimation Estimate states of a nonlinear, non-stationary state space model State dynamic equation x k = 0.5x k 1 + Measurement equation 25x k 1 (1 + x 2 k 1 ) + cos(1.2(k 1)) + w k y k = x2 k 20 + v k w k and v k are zero-mean, Gaussian white noise. 30 / 55

31 Introduction Example - 1D Estimation State estimate State (x k ) of a 1D model True value Posterior mean estimate Time Posterior using s Posterior density Time Sample space / 55

32 Introduction Results Tracking result 32 / 55

33 Outline Introduction 1 Introduction / 55

34 Design Hierarchy Introduction Various stages in developing and implementing a particle filter algorithm 34 / 55

35 Design Hierarchy Introduction Various stages in developing and implementing a particle filter algorithm 35 / 55

36 Introduction DSP Floating-point DSP TI C6713 Internal memory (IRAM) or External memory (SDRAM) Speed Size Sampling rate of 0.3 seconds, for K = 1, N = 1000, 225 MHz Table: Memory sizes in the C6713-DSK - Single-target, N = 1000 Type Section Available Occupied Internal IRAM 192 KB KB 36 / 55

37 Introduction FPGA Realization Matlab simulation Finite word length identified from Matlab fixed-point simulation Xilinx System generator - Simulink based tool Xilinx blocks - bit and cycle true FPGA code Nonlinear functions using CORDIC algorithm Device: Xilinx Virtex II Pro Xilinx Embedded Development Kit (EDK) To use MicroBlaze soft-core and Power PC hard-core 37 / 55

38 Particle State Update

39 Particle Proposal Stage

40 Data Likelihood Evaluation

41 Particle Weight Evaluation Stage

42 Reampling Stage Resampling Compute number of replications based on particle weight Control circuitry and logic functions 1: U U[0, 1] 2: K = M/W n 3: ind r = 0, ind d = M 1 4: for m=1 to M do 5: temp = w (m) n Figure: Residual Systematic Resampling.K U 6: r indr = temp 7: U = temp r indr 8: if r indr > 0 then 9: i (indr ) r = m, ind r = ind r : else 11: i (ind d) d = m, ind d = ind d 1 12: end if 13: end for

43 Introduction Approximation for Data Likelihood Data likelihood ( p(y(t) x k (t)) M 1 Q 1+ 1 κ P exp m=0 2πσ θ 2κλ p i DOA component of nonlinear state transition ( ( h θ mτ (x k (t)) y t+mτ (p i ) ) 2 2σ 2 θ h θ mτ(x k (t))=arctan sin(θ k (t))+t cos(φ k (t))e(q k (t)) cos(θ k (t))+t sin(φ k (t))e (Q k (t)) Laplace method to approximate distribution Newton search Jacobians and Hessians )) 43 / 55

44 Introduction Newton search in FPGA (1/2) Hardware implementation of square-root and division use Newton-Raphson search Difficulty in using Newton search to identify mode Cost function, Hessian, and Gradient evaluation - nonlinear functions Software implementation in floating-point - MicroBlaze soft-core or Power PC hard-core processor Avoids sensitivity to word length Accelerated by configurable hardware 44 / 55

45 Introduction Newton search in FPGA (2/2) MicroBlaze - soft-core IP (processor) Configurable - has 64 kb RAM for code and data Floating-point unit (FPU) - accelerates floating-point operations Power PC hard-core IP (processor) Non-Configurable - has 128 kb for data and 64 kb for code Floating-point operations are emulated in software Newton search for identifying mode - code size > 100 kb 45 / 55

46 FPGA - Resource Utilization Batch-based tracker utilizes at least four times more resources Excluding Newton-search Requires large, recent FPGA device Table: Resource utilization - Batch-based tracker PF Stage Resource Proposal Weight evaluation Resampling Overall # Slices a a For comparison, a 16 bit multiplier uses 153 slices Table: Resource utilization - Bearings-only tracker PF Stage Resource Proposal Weight evaluation Resampling Overall # Slices

47 FPGA - Latency Introduction Table: Update rate using FPGA Clock frequency of 100 MHz Batch-based tracker Bearings-only tracker Latency 3N N + 50 Update rate N = 200 N = 1000 N = 200 N = µs 33 µs 3.5 µs 30 µs Update rate of 9 µs is sufficient to generate estimates every T = 1 s 47 / 55

48 DSP Results Complete batch-based particle filter algorithm on TI C6713 Figure: Target DOAs Figure: Target x-y tracks Floating point DSP Matlab Generic C Ground truth θ in [ ] 0 y time [sec] x

49 FPGA Results Measurements Propose particles Evaluate weights Estimate states Resample particles Matlab ModelSim Figure: FPGA simulation setup.

50 FPGA Results FPGA implementation of data likelihood evaluated in ModelSim Figure: Target DOAs Figure: Target x-y tracks θ in [ ] y FPGA estimate Matlab estimate Ground truth Sensor time [sec] x

51 Comparison Introduction Comparison of implementation strategies for particle filters Characteristic DSP FPGA Accuracy High High Speed Medium High Power dissipation High High flexibility High Medium 51 / 55

52 Discussion (1/2) Introduction Particle filter characteristics Number of particles N can be large - complexity Parallel computations for individual particles Resampling can prevent parallelization - parallel resampling Nonlinear functions Constraints Real-time operation Low power Accuracy - word length 52 / 55

53 Discussion (2/2) Introduction Digital implementation of particle filter Multiple FPGAs SIMD architectures General purpose Graphical processing units (GP-GPUs) Random number generators Analog or mixed-mode implementation of particle filter Nonlinear functions in analog - arctan, Gaussian Random number or noise generation 53 / 55

54 Acknowledgements and References Thanks to Advisor Prof.James H. McClellan, Georgia Tech. Dr.Volkan Cevher, Rice University. SMC Webpage: A. Doucet and N. Freitas and N. Gordon Sequential Monte Carlo Methods in Practice. Springer-Verlag, V. Cevher, R. Velmurugan, and J. H. McClellan Acoustic Multi- using Direction-of-Arrival Batches IEEE Tran. Signal Processing, June 2007.

55 Thanks! Introduction Questions 55 / 55

56 Sequential Importance Sampling (SIS) Illustration Source: Michael Isard s CONDENSATION demos 56 / 55

57 CORDIC Algorithm COordinate Rotation DIgital Computer (CORDIC) algorithm x n+1 = x n md n y n 2 σ(n) y n+1 = y n + d n x n 2 σ(n) z n+1 = z n w σ(n) Type m σ n d n = signz n d n = signy n x n K(x 0 cos z 0 y 0 sin z 0 ) x n K p x0 circular 2 + y n y n K(y 0 cos z 0 + x 0 sin z 0 ) y n 0 tan 1 2 k z n 0 z n z 0 tan 1 y 0 x 0 x n K (x 1 cosh z 1 + y 1 sinh z 1 ) x n K p x0 hyperbolic 2 + y n k y n K (y 1 cosh z 1 + x 1 sinh z 1 ) y n 0 tanh 1 2 k z n 0 z n z 0 tanh 1 y 0 x 0 57 / 55

58 FPGA Device Detail Table: Xilinx Virtex II Pro FPGA - XC2VP30 Resource # Logic cells a # Slices b Block RAM Clock frequency kb 100 MHz a Logic cell one 4-input LUT + one Flip-Flop + Carry logic b Each slice includes two 4-input function generators, carry logic, arithmetic logic gates, wide function multiplexers, and two storage elements. 58 / 55