Statistical Filtering and Control for AI and Robotics Part III. - PowerPoint PPT Presentation

Statistical Filtering and Control for AI and Robotics Part III. Extended Kalman filter, Particle filter, etc Riccardo Muradore 1 / 30

Reference This lecture is based on the following book Sebastian Thrun, Wolfram Burgard and Dieter Fox, “Probabilistic Robotics”, MIT Press, 2005 Several pictures from this book have been copied and pasted here 2 / 30

Contents Introduction Extended Kalman filter Non parametric filtering: the Histogram filter and the Particle filter 3 / 30

Introduction 4 / 30

Bayer recursive algorithm Algorithm 1 bel ( x t ) =BayerFilter( bel ( x t − 1 ) , u t , z t ) 1: forall x t ∈ S x do � bel ( x t ) = S x p ( x t | u t , x t − 1 ) bel ( x t − 1 ) dx t − 1 2: bel ( x t ) = η p ( z t | x t ) bel ( x t ) 3: 4: end 5: return bel ( x t ) ◮ Line 2: prediction bel ( x t ) computed using the old bel ( x t − 1 ) and the current controls u t ◮ Line 3: update bel ( x t ) computed using bel ( x t ) and the new measurements z t 5 / 30

Kalman filter Algorithm 2 [ˆ x k +1 | k +1 , P k +1 | k +1 ] =KalmanFilter(ˆ x k | k , P k | k , u k , y k +1 ) ˆ x k +1 | k = A ˆ x k | k + Bu k 1: P k +1 | k = AP k | k A T + Q 2: ˆ x k +1 | k +1 = ˆ x k +1 | k + K k +1 ( y k +1 − C ˆ x k +1 | k ) 3: P k +1 | k +1 = P k +1 | k − P k +1 | k C T ( CP k +1 | k C T + R ) − 1 CP k +1 | k 4: 5: return ˆ x k +1 | k +1 , P k +1 | k +1 ◮ Line 1-2: prediction bel ( x k +1 ) computed using the old bel ( x k ) and the current controls u k ◮ Line 3-4: update bel ( x k +1 ) computed using bel ( x k +1 ) and the new measurements z k +1 With linear Gaussian models ◮ bel ( x t ) is fully described by the mean ˆ x k +1 | k and the variance P k +1 | k ◮ bel ( x t ) is fully described by the mean ˆ x k | k and the variance P k | k 6 / 30

Kalman filter The Kalman filter/predictor is extremely efficient be- cause it is based on two strong assumptions: 1. the state equation and the measurement equation are linear 2. the random variables are Gaussian 7 / 30

Kalman filter What’s happen when the assumption “ the state equation and the measurement equation are linear ” is not true? 8 / 30

Extended Kalman filter 9 / 30

Extended Kalman filter With arbitrary nonlinear functions f and h , = f ( x t , u t ) + w t x t +1 = h ( x t ) + v t y t the belief is no longer Gaussian and there is not a closed-form solution. The extended Kalman filter (EKF) calculates an approximation to the true belief by a Gaussian. In particular, the belief bel ( x t ) at time t is represented by a mean and a covariance 10 / 30

Extended Kalman filter Linearization of y = g ( x ) via Taylor expansion around µ � g ( x ) = g ( µ ) + ∂ g � ( x − µ ) + higher orders � ∂ x � x = µ 11 / 30

Extended Kalman filter Algorithm 3 [ˆ x k +1 | k +1 , P k +1 | k +1 ] =EKF(ˆ x k | k , P k | k , u k , y k +1 ) ˆ x k +1 | k = f (ˆ x k | k , u k ) 1: � A k = ∂ f ( x , u ) 2: � ∂ x � x =ˆ x k | k , u = u k P k +1 | k = A k P k | k A T k + Q 3: � C k = ∂ h ( x ) 4: � ∂ x � x =ˆ x k +1 | k � − 1 K k +1 = P k +1 | k C T C k P k +1 | k C T � k + R 5: k ˆ x k +1 | k +1 = ˆ x k +1 | k + K k +1 ( y k +1 − h (ˆ x k +1 | k )) 6: − P k +1 | k C T k ( C k P k +1 | k C T P k +1 | k +1 = P k +1 | k + 7: k R ) − 1 C k P k +1 | k 8: return ˆ x k +1 | k +1 , P k +1 | k +1 12 / 30

Extended Kalman filter When the EKF works well: 1. f and h are approximately linear 2. the statistical description is not multi-modal Observation: the less certain is the knowledge (i.e. high variance), the wider the Gaussian belief 13 / 30

Extended Kalman filter good approximation bad approximation 14 / 30

Non parametric filtering: the Histogram filter and the Particle filter 15 / 30

Non parametric filtering What’s happen when also the assumption “ the random variables are Gaussian ” is not true? We have to resort to nonparametric filters: filters that do not rely on a fixed functional form of the posterior (e.g. Gaussian) Q. How do they work? A. the probability are approximated by a finite number of values properly selected (each value corresponds to a region in state space) The quality of the approximation depends on the number of parameters used to represent the probability! ADVANTAGE nonparametric filters are well-suited to represent complex multimodal beliefs DRAWBACK increased computational complexity 16 / 30

The Histogram Filter Histogram filters decom- pose a continuous state space with support S t into finitely many regions ( K ) x t ∈ { ξ 1 , t , ξ 2 , t , . . . , ξ K , t } where K � ξ k , t = S t k =1 ξ i , t ∩ ξ j , t = ∅ , ∀ i � = j Continuous random variable − → Discrete random variable 17 / 30

The Histogram Filter Through the granularity of the decomposition, we can trade off accuracy and computational efficiency. Fine-grained decompositions infer smaller approximation errors than coarse ones, but at the expense of increased computational complexity. Each region ξ k , t has a probability assigned p k , t , then p ( x t ) = p k , t x t ∈ ξ k , t ⇒ | ξ k , t | What happens to the PDFs p ( x t | u t , x t − 1 ) − → p ( ξ k , t | u t , ξ i , t − 1 ) =??? p ( z t | x t ) − → p ( z t | ξ k , t ) =??? 18 / 30

The Histogram Filter Educated Guess: the densities in each region ξ k , t is approximated by the density of a particular element within that region, e.g. the mean � x k , t = | ξ k , t | − 1 ¯ x t dt ξ k , t Then we have η p ( x t | u t , x t − 1 ) − → p ( ξ k , t | u t , ξ i , t − 1 ) ≃ | ξ k , t | p (¯ x k , t | u t , ¯ x i , t − 1 ) p ( z t | x t ) − → p ( z t | ξ k , t ) ≃ p ( z t | ¯ x k , t ) 19 / 30

The Histogram Filter Algorithm 4 { p k , t } K 1 =HF( { p k , t − 1 } K 1 , u t , z t ) 1: for k = 1 to K do p k , t = � K x k | X t − 1 = ¯ i =1 p ( X t = ¯ x i , u t ) p i , t − 1 2: p k , t = η p ( z t | X t = ¯ x k ) p k , t 3: 4: end 20 / 30

Particle Filter Also particle filters approximate the posterior by a finite number of parameters but the parameters representing the posterior bel ( x t ) are a set of random state samples drawn from the previous posterior Like histogram filters, particle filters can represent a much broader space of distributions than Gaussian 21 / 30

Particle Filter x [ i ] The samples t , i = 1 , . . . , M of a posterior distribution are called particles X t = { x [1] t , x [2] t , . . . , x [ M ] } t The likelihood that a state hypothesis x t belongs to X t should be proportional to the Bayes filter posterior bel ( x t ) x [ m ] ∼ p ( x t | z 1: t , u 1: t ) t 22 / 30

Particle Filter Algorithm 5 X t =ParticleFilter( X t − 1 , u t , z t ) ¯ X t = X t = ∅ 1: 2: for m = 1 to M do sample x [ m ] ∼ p ( x t | x [ m ] t − 1 , u t ) 3: t w [ m ] = p ( z t | x [ m ] ) 4: t t X t ∪ � x [ m ] , w [ m ] X t = ¯ ¯ � 5: t t 6: end 7: for m = 1 to M do draw i with probability ∝ w [ i ] 8: t add x [ i ] to X t 9: t 10: end 11: return X t 23 / 30

Particle Filter Algorithm 6 X t =ParticleFilter( X t − 1 , u t , z t ) ¯ X t = X t = ∅ 1: 2: for m = 1 to M do sample x [ m ] ∼ p ( x t | x [ m ] t − 1 , u t ) 3: t w [ m ] = p ( z t | x [ m ] ) 4: t t X t ∪ � x [ m ] , w [ m ] X t = ¯ ¯ � 5: t t 6: end 7: for m = 1 to M do draw i with probability ∝ w [ i ] 8: t add x [ i ] to X t 9: t 10: end 11: return X t Generation of new M samples x [ m ] using the state transition distribution t p ( x t | x t − 1 , u t ) based on particles in X t − 1 and the current control u t 24 / 30

Particle Filter Algorithm 7 X t =ParticleFilter( X t − 1 , u t , z t ) ¯ X t = X t = ∅ 1: 2: for m = 1 to M do sample x [ m ] ∼ p ( x t | x [ m ] t − 1 , u t ) 3: t w [ m ] = p ( z t | x [ m ] ) 4: t t X t ∪ � x [ m ] , w [ m ] X t = ¯ ¯ � 5: t t 6: end 7: for m = 1 to M do draw i with probability ∝ w [ i ] 8: t add x [ i ] to X t 9: t 10: end 11: return X t For each particle x [ m ] , compute the importance factor w [ m ] based on the t t measurement probability p ( z t | x t ) and the new measurement z t 25 / 30

Particle Filter Algorithm 8 X t =ParticleFilter( X t − 1 , u t , z t ) ¯ X t = X t = ∅ 1: 2: for m = 1 to M do sample x [ m ] ∼ p ( x t | x [ m ] t − 1 , u t ) 3: t w [ m ] = p ( z t | x [ m ] ) 4: t t X t ∪ � x [ m ] , w [ m ] X t = ¯ ¯ � 5: t t 6: end 7: for m = 1 to M do draw i with probability ∝ w [ i ] 8: t add x [ i ] to X t 9: t 10: end 11: return X t Resampling step: replace ¯ X t with another set of the same dimension M , X t 26 / 30

Particle Filter Algorithm 9 X t =ParticleFilter( X t − 1 , u t , z t ) ¯ X t = X t = ∅ 1: 2: for m = 1 to M do sample x [ m ] ∼ p ( x t | x [ m ] t − 1 , u t ) 3: t w [ m ] = p ( z t | x [ m ] ) 4: t t X t ∪ � x [ m ] , w [ m ] X t = ¯ ¯ � 5: t t 6: end 7: for m = 1 to M do draw i with probability ∝ w [ i ] 8: t add x [ i ] to X t 9: t 10: end 11: return X t ¯ X t represents bel ( x t ) 27 / 30