Computer Vision 2 Exercise 2. Extended Kalman Filter & Particle Filter

Transcription

1 Computer Vision Exercise Extended Kalman Filter & Particle Filter RWTH Aachen University, Computer Vision Group

2 Content Exercise Question : Extended Kalman Filter Compared to basic KF Unicycle motion model Nonlinearities & Jacobians Filtered trajectory from noisy measurements 5 Question : Particle Filter Compared to general KF SIR Algorithm Implementation Details Computer Vision - Exercise - EKF & Particle Filter

3 Question : Extended Kalman Filter

4 Q: EKF - Compared to basic KF Kalman Filter Extended Kalman Filter Computer Vision - Exercise - EKF & Particle Filter

5 Q: EKF - Unicycle Motion Model The unicycle motion model is an approximation often used for bicycle or car motions. State vector: 5 Example Trajectory position x position y. orientation angle.. y velocity y x x 5 Computer Vision - Exercise - EKF & Particle Filter

6 Q: EKF a) Point out, which steps contain nonlinearities and give the definition of the functions g and h. Dynamic Model x t = g(x t )+ t nonlinear functions Measurement Model y t = h(x t )+ t t t t t x t g : R! R, 6y t 7 t 5 7! v t x t + 6y t + tv t cos t x t apple tv t sin t 7 t 5 h : R! R, 6y t 7 t 5 7! xt y t v t v t 6 Computer Vision - Exercise - EKF & Particle Filter

7 Q: EKF b) b) Compute the Jacobians Gt and Ht of g(x) and h(x) respectively. x t g : R! R, 6y t 7 t 5 7! v t G = H @x t x t + 6y t t tv t cos t x t apple tv t sin t 7 t 5 h : R! R, 6y t 7 t 5 7! xt y t v t v t 7 5 = # = tv t sin t tcos t 6 tv t cos tsin t t7 5 apple same as in linear case 7 Computer Vision - Exercise - EKF & Particle Filter

8 Q: EKF c) c) Implement extended Kalman filter. Generate measurements by adding Gaussian noise to original state at each time step. Use EKF to estimate original trajectory based on noisy measurements. 5 Original trajectory with noisy measurements y y x x 8 Computer Vision - Exercise - EKF & Particle Filter

9 Q: EKF c) c) Implement extended Kalman filter. 5 Filtered trajectory from noisy measurements Original trajectory Measurements Filtered Trajectory Computer Vision - Exercise - EKF & Particle Filter

10 Q: EKF c) c) Implement extended Kalman filter. dt Influence of and mt 5 Filtered trajectory from noisy measurements 5 Filtered trajectory from noisy measurements sigma_d = eye() *.; sigma_m = eye() *.5; sigma_d = eye() *.; sigma_m = eye() *.5; Computer Vision - Exercise - EKF & Particle Filter

11 Question : Particle Filter

12 Q: Particle Filter What is different from particle filters to Kalman filters? Kalman Filter: all probability distributions are normal distributions. Particle Filter: any distribution is possible. In particular, this allows us to model multiple hypothesis for the state. Normal Distribution Arbitrary Distribution Computer Vision - Exercise - EKF & Particle Filter

13 Q: Particle Filter What is different from particle filters to Kalman filters? In particular, this allows us to model multiple hypothesis for the state. Kalman Filter: Single Mode Particle Filter: Multiple Modes Computer Vision - Exercise - EKF & Particle Filter

14 Q: Particle Filter SIR (Sampling Importance Resampling) Number of particles Particle State transition distribution, according to dynamic model here: constant position model Weight of particle Re-sampling Sampling Set of particles (samples from posterior at time t) Particles x Detections y 5 5 Resampling Computer Vision - Exercise - EKF & Particle Filter 5 5

15 Q: Particle Filter SIR (Sampling Importance Resampling) a) generate_particles.m b) compute_particle_likelihood.m c) inverse_transform_sampling.m 5 Computer Vision - Exercise - EKF & Particle Filter

16 Q: Particle Filter a) Generate particles (samples) Draw random samples from a D Normal distribution. MATLABs randn returns normally distributed samples. Probability Density D Normal Distribution Drawn Samples Computer Vision - Exercise - EKF & Particle Filter - -

17 Q: Particle Filter a) Generate particles (samples) Draw random samples from a D Normal distribution. MATLABs randn returns normally distributed samples Computer Vision - Exercise - EKF & Particle Filter 5 6 7

18 Q: Particle Filter b) Compute particle likelihood (weights) How likely does a particle correspond to a detection? Measure: Parzen density estimation with Gaussian kernel wti = wti j xit yjt X Computer Vision - Exercise - EKF & Particle Filter 5 5 yj exp ( x xix x + yjy xiy ) y

19 Q: Particle Filter - Resampling Inverse transform sampling Goal: resample N particles from existing set of N particles, favor particles with larger weight.. From discrete particle distribution compute cumulative distribution. Each bin corresponds to a particle, the height of the bin corresponds to the weight.. Sample u from uniform distribution between and. Look up bin in cumulative distribution and pick resulting particle x j 9 Computer Vision - Exercise - EKF & Particle Filter

20 Q: Particle Filter - Result Computer Vision - Exercise - EKF & Particle Filter