An introduction to particle filters

Transcription

1 An introduction to particle filters Andreas Svensson Department of Information Technology Uppsala University June 10, 2014 June 10, 2014, 1 / 16 Andreas Svensson - An introduction to particle filters

2 Outline Motivation and ideas Algorithm High-level Matlab code Practical aspects Resampling Computational complexity Software Terminology Advanced topics Convergence Extensions References June 10, 2014, 2 / 16 Andreas Svensson - An introduction to particle filters

3 State Space Models June 10, 2014, 3 / 16 Andreas Svensson - An introduction to particle filters

4 State Space Models Linear Gaussian state space model x t+1 = Ax t + Bu t + w t y t = Cx t + Du t + e t with w t and e t Gaussian and E [ w t w T t ] = Q, E [ et e T t ] = R. June 10, 2014, 3 / 16 Andreas Svensson - An introduction to particle filters

5 State Space Models Linear Gaussian state space model x t+1 = Ax t + Bu t + w t y t = Cx t + Du t + e t with w t and e t Gaussian and E [ w t wt T ] [ = Q, E et et T ] = R. A more general state space model in different notation x t+1 f (x t+1 x t, u t ) y t g(y t x t, u t ) June 10, 2014, 3 / 16 Andreas Svensson - An introduction to particle filters

6 Filtering - Find p(x t y 1:t ) June 10, 2014, 4 / 16 Andreas Svensson - An introduction to particle filters

7 Filtering - Find p(x t y 1:t ) Linear Gaussian problems: June 10, 2014, 4 / 16 Andreas Svensson - An introduction to particle filters

8 Filtering - Find p(x t y 1:t ) Linear Gaussian problems: Kalman filter! June 10, 2014, 4 / 16 Andreas Svensson - An introduction to particle filters

9 Filtering - Find p(x t y 1:t ) Linear Gaussian problems: Kalman filter! Nonlinear problems: June 10, 2014, 4 / 16 Andreas Svensson - An introduction to particle filters

10 Filtering - Find p(x t y 1:t ) Linear Gaussian problems: Kalman filter! Nonlinear problems: EKF, UKF, Particle filter June 10, 2014, 4 / 16 Andreas Svensson - An introduction to particle filters

11 Filtering - Find p(x t y 1:t ) Linear Gaussian problems: Kalman filter! ''Exact solution to the right problem'' Nonlinear problems: EKF, UKF, Particle filter June 10, 2014, 4 / 16 Andreas Svensson - An introduction to particle filters

12 Filtering - Find p(x t y 1:t ) Linear Gaussian problems: Kalman filter! ''Exact solution to the right problem'' Nonlinear problems: EKF, UKF, ''Exact solution to the wrong problem'' Particle filter June 10, 2014, 4 / 16 Andreas Svensson - An introduction to particle filters

13 Filtering - Find p(x t y 1:t ) Linear Gaussian problems: Kalman filter! ''Exact solution to the right problem'' Nonlinear problems: EKF, UKF, ''Exact solution to the wrong problem'' Particle filter ''Approximate solution to the right problem'' June 10, 2014, 4 / 16 Andreas Svensson - An introduction to particle filters

14 Idea True state tracejctory State Time A linear system - Find p(x t y 1:t) (true x t shown)! June 10, 2014, 5 / 16 Andreas Svensson - An introduction to particle filters

15 Idea True state tracejctory Kalman filter mean State Time Kalman filter mean June 10, 2014, 5 / 16 Andreas Svensson - An introduction to particle filters

16 Idea True state tracejctory Kalman filter mean Kalman filter covariance State Time Kalman filter mean and covariance June 10, 2014, 5 / 16 Andreas Svensson - An introduction to particle filters

17 Idea True state tracejctory Kalman filter mean Kalman filter covariance State Time Kalman filter mean and covariance defines a Gaussian distribution at each t June 10, 2014, 5 / 16 Andreas Svensson - An introduction to particle filters

18 Idea True state tracejctory State Time A numerical approximation can be used to describe the distribution - Particle Filter June 10, 2014, 5 / 16 Andreas Svensson - An introduction to particle filters

19 Idea True state tracejctory State Time The Particle Filter can easily handle, e.g., non-gaussian multimodal hypotheses June 10, 2014, 5 / 16 Andreas Svensson - An introduction to particle filters

20 Idea Generate a lot of hypotheses about x t, keep the most likely ones and propagate them further to x t+1. Keep the likely hypotheses about x t+1, propagate them again to x t+2, etc. June 10, 2014, 6 / 16 Andreas Svensson - An introduction to particle filters

21 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T end 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling x i t+1 from f ( xi t, u t ) N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 7 / 16 Andreas Svensson - An introduction to particle filters

22 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T end 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling x i t+1 from f ( xi t, u t ) N Number of particles, x i t Particles, w i t Particle weights 0 x(:,1) = random(i_dist,n,1); w(:,1) = ones(n,1)/n; for t = 1:T end 1 w(:,t) = pdf(m_dist,y(t)-g(x(:,t))); w(:,t) = w(:,t)/sum(w(:,t)); 2 Resample x(:,t) 3 x(:,t+1) = f(x(:,t),u(t)) + random(t_dist,n,1); June 10, 2014, 7 / 16 Andreas Svensson - An introduction to particle filters

23 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T end 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

24 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

25 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

26 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

27 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

28 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

29 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

30 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

31 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

32 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

33 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

34 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

35 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

36 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters State x Time

37 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights State x Time June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters

38 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N end N Number of particles, x i t Particles, w i t Particle weights State x Time June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters

39 Algorithm 0 Initialize x1 i p(x 1) and w i = 1 N for t = 1 to T end 1 Evaluate w i t = g(y t x i t, u t ) 2 Resample {x i t } N i=1 from {x i t, w i t} N i=1 3 Propagate x i t by sampling from f ( x i t 1, u t 1) for i = 1,..., N N Number of particles, x i t Particles, w i t Particle weights State x Time June 10, 2014, 8 / 16 Andreas Svensson - An introduction to particle filters

40 Resampling June 10, 2014, 9 / 16 Andreas Svensson - An introduction to particle filters

41 Resampling Represent the information contained in the N black dots (of different sizes) state June 10, 2014, 9 / 16 Andreas Svensson - An introduction to particle filters

42 Resampling Represent the information contained in the N black dots (of different sizes) with the N red dots (of equal sizes) state June 10, 2014, 9 / 16 Andreas Svensson - An introduction to particle filters

43 Resampling Represent the information contained in the N black dots (of different sizes) with the N red dots (of equal sizes) state Can be seen as sampling from a cathegorical distribution June 10, 2014, 9 / 16 Andreas Svensson - An introduction to particle filters

44 Resampling Represent the information contained in the N black dots (of different sizes) with the N red dots (of equal sizes) state Can be seen as sampling from a cathegorical distribution To avoid particle depletion (''a lot of 0-weights particles'') June 10, 2014, 9 / 16 Andreas Svensson - An introduction to particle filters

45 Resampling Represent the information contained in the N black dots (of different sizes) with the N red dots (of equal sizes) state Can be seen as sampling from a cathegorical distribution To avoid particle depletion (''a lot of 0-weights particles'') Some Matlab code: v = rand(n,1); wc = cumsum(w(:,t)); [,ind1] = sort([v;wc]); ind=find(ind1<=n)-(0:n-1)'; x(:,t)=x(ind,t); w(:,t)=ones(n,1)./n; June 10, 2014, 9 / 16 Andreas Svensson - An introduction to particle filters

46 Resampling cont'd June 10, 2014, 10 / 16 Andreas Svensson - An introduction to particle filters

47 Resampling cont'd Crucial step in the development in the 90's for making particle filters useful in practice June 10, 2014, 10 / 16 Andreas Svensson - An introduction to particle filters

48 Resampling cont'd Crucial step in the development in the 90's for making particle filters useful in practice Many different techniques exists with different properties and effiency (the Matlab code shown was only one way of doing it) June 10, 2014, 10 / 16 Andreas Svensson - An introduction to particle filters

49 Resampling cont'd Crucial step in the development in the 90's for making particle filters useful in practice Many different techniques exists with different properties and effiency (the Matlab code shown was only one way of doing it) Computationally heavy. Not necessary to do in every iteration - a ''depletion measure'' can be introduced, and the resampling only performed when it reaches a certain threshold. June 10, 2014, 10 / 16 Andreas Svensson - An introduction to particle filters

50 Resampling cont'd Crucial step in the development in the 90's for making particle filters useful in practice Many different techniques exists with different properties and effiency (the Matlab code shown was only one way of doing it) Computationally heavy. Not necessary to do in every iteration - a ''depletion measure'' can be introduced, and the resampling only performed when it reaches a certain threshold. It is sometimes preferred to use the logarithms of the weights for numerical reasons. June 10, 2014, 10 / 16 Andreas Svensson - An introduction to particle filters

51 Computational complexity June 10, 2014, 11 / 16 Andreas Svensson - An introduction to particle filters

52 Computational complexity In theory O(NTn 2 x) (n x is the number of states) June 10, 2014, 11 / 16 Andreas Svensson - An introduction to particle filters

53 Computational complexity In theory O(NTn 2 x) (n x is the number of states) (Kalman filter is approx. of order O(Tn 3 x)) June 10, 2014, 11 / 16 Andreas Svensson - An introduction to particle filters

54 Computational complexity In theory O(NTn 2 x) (n x is the number of states) (Kalman filter is approx. of order O(Tnx)) 3 Possible bottlenecks: Resampling step Likelihood evaluation (for the weight evaluation) Sampling from f for the propagation (trick: use proposal distributions!) June 10, 2014, 11 / 16 Andreas Svensson - An introduction to particle filters

55 Software June 10, 2014, 12 / 16 Andreas Svensson - An introduction to particle filters

56 Software Lot of software packages exists. However, to my knowledge, no package that ''everybody'' uses. (In our research, we write our own code.) June 10, 2014, 12 / 16 Andreas Svensson - An introduction to particle filters

57 Software Lot of software packages exists. However, to my knowledge, no package that ''everybody'' uses. (In our research, we write our own code.) A few relevant existing packages: Python: pyparticleest Matlab: PFToolbox, PFLib C++: Particle++ Disclaimer: I have not used any of these packages myself June 10, 2014, 12 / 16 Andreas Svensson - An introduction to particle filters

58 Terminology June 10, 2014, 13 / 16 Andreas Svensson - An introduction to particle filters

59 Terminology Bootstrap particle filter ''Standard'' particle filter June 10, 2014, 13 / 16 Andreas Svensson - An introduction to particle filters

60 Terminology Bootstrap particle filter ''Standard'' particle filter Sequential Monte Carlo (SMC) Particle filter June 10, 2014, 13 / 16 Andreas Svensson - An introduction to particle filters

61 Convergence June 10, 2014, 14 / 16 Andreas Svensson - An introduction to particle filters

62 Convergence The particle filter is ''exact for N = '' June 10, 2014, 14 / 16 Andreas Svensson - An introduction to particle filters

63 Convergence The particle filter is ''exact for N = '' Consider a function g(x t ) of interest. How well can it be approximated as ĝ(x t ) when x t is estimated in a particle filter? June 10, 2014, 14 / 16 Andreas Svensson - An introduction to particle filters

64 Convergence The particle filter is ''exact for N = '' Consider a function g(x t ) of interest. How well can it be approximated as ĝ(x t ) when x t is estimated in a particle filter? E [ĝ(x t ) E [g(x t )]] 2 pt g(xt) sup N June 10, 2014, 14 / 16 Andreas Svensson - An introduction to particle filters

65 Convergence The particle filter is ''exact for N = '' Consider a function g(x t ) of interest. How well can it be approximated as ĝ(x t ) when x t is estimated in a particle filter? E [ĝ(x t ) E [g(x t )]] 2 pt g(xt) sup N If the system ''forgets exponentially fast'' (e.g. linear systems), and some additional weak assumptions, p t = p <, i.e., E [ĝ(x t ) E [g(x t )]] 2 C N June 10, 2014, 14 / 16 Andreas Svensson - An introduction to particle filters

66 Extensions June 10, 2014, 15 / 16 Andreas Svensson - An introduction to particle filters

67 Extensions Smoothing: Finding p(x t y 1:T ) (marginal smoothing) or p(x 1:t y 1:T ) (joint smoothing) instead of p(x t y 1:t ) (filtering). Offline (y 1:T has to be available). Increased computational load. June 10, 2014, 15 / 16 Andreas Svensson - An introduction to particle filters

68 Extensions Smoothing: Finding p(x t y 1:T ) (marginal smoothing) or p(x 1:t y 1:T ) (joint smoothing) instead of p(x t y 1:t ) (filtering). Offline (y 1:T has to be available). Increased computational load. If f (x t+1 x t, u t ) is not suitable to sample from, proposal distributions can be used. In fact, an optimal proposal (w.r.t. reduced variance) exists. June 10, 2014, 15 / 16 Andreas Svensson - An introduction to particle filters

69 Extensions Smoothing: Finding p(x t y 1:T ) (marginal smoothing) or p(x 1:t y 1:T ) (joint smoothing) instead of p(x t y 1:t ) (filtering). Offline (y 1:T has to be available). Increased computational load. If f (x t+1 x t, u t ) is not suitable to sample from, proposal distributions can be used. In fact, an optimal proposal (w.r.t. reduced variance) exists. Rao-Blackwellization for mixed linear/nonlinear models. June 10, 2014, 15 / 16 Andreas Svensson - An introduction to particle filters

70 Extensions Smoothing: Finding p(x t y 1:T ) (marginal smoothing) or p(x 1:t y 1:T ) (joint smoothing) instead of p(x t y 1:t ) (filtering). Offline (y 1:T has to be available). Increased computational load. If f (x t+1 x t, u t ) is not suitable to sample from, proposal distributions can be used. In fact, an optimal proposal (w.r.t. reduced variance) exists. Rao-Blackwellization for mixed linear/nonlinear models. System identification: PMCMC, SMC 2. June 10, 2014, 15 / 16 Andreas Svensson - An introduction to particle filters

71 References Gustafsson, F. (2010). Particle filter theory and practice with positioning applications. Aerospace and Electronic Systems Magazine, IEEE, 25(7), Schön, T.B., & Lindsten, F. Learning of dynamical systems - Particle Filters and Markov chain methods. Draft available. Doucet, A., & Johansen, A. M. (2009). A tutorial on particle filtering and smoothing: Fifteen years later. Handbook of Nonlinear Filtering, 12, Homework: Implement your own Particle Filter for any (simple) problem of your choice! June 10, 2014, 16 / 16 Andreas Svensson - An introduction to particle filters