An introduction to particle filters Andreas Svensson Department of - PowerPoint PPT Presentation

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

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

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

A more general state space model in different notation x t u t g y t x t u t t e t e T y t f x t w t w T t x t 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 � � � � = R . = Q , E June 10, 2014, 3 / 16 Andreas Svensson - An introduction to particle filters

w t w T e t e T t t 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 � � � � = R . = Q , E ◮ 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

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'' Filtering - Find p ( x t | y 1: t ) June 10, 2014, 4 / 16 Andreas Svensson - An introduction to particle filters

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

''Exact solution to the right problem'' Nonlinear problems: EKF, UKF, … ''Exact solution to the wrong problem'' Particle filter ''Approximate solution to the right problem'' 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

''Exact solution to the right problem'' EKF, UKF, … ''Exact solution to the wrong problem'' Particle filter ''Approximate solution to the right problem'' 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

''Exact solution to the right problem'' ''Exact solution to the wrong problem'' ''Approximate solution to the right problem'' 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

''Exact solution to the wrong problem'' ''Approximate solution to the right problem'' 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

''Approximate solution to the right problem'' 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

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

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

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

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

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

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

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

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

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

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

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

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

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

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