A Comparison of the EKF, SPKF, and the Bayes Filter for Landmark-Based Localization

Transcription

1 A Comparison of the EKF, SPKF, and the Bayes Filter for Landmark-Based Localization and Timothy D. Barfoot CRV 2

2 Outline Background Objective Experimental Setup Results Discussion Conclusion 2

3 Outline Background Objective Experimental Setup Results Discussion Conclusion 3

4 Background What is state estimation? The problem of estimating the state of a process Noisy measurements Sensor fusion Sample applications Robot localization and mapping Chemical process control Weather prediction 4

5 Background State Space Models Discrete-time, nonlinear models: x k = h (x k, u k, w k ) y k = g (x k, n k ) Markov assumption: u u k u k x h x h x k h x k w w k w k n g g g n k n k y y k y k 5

6 Background Bayes Filter p(x k u :k, y :k ) }{{} posterior belief = η p(y k x k ) }{{} observation correction using g( ) p(x k x k, u k ) }{{} motion prediction using h( ) p(x k u :k, y :k ) }{{} prior belief dx k Intractable Infinite-dimensional Infinite computation time 6

7 Background Bayes Filter Infinite Dimension Kalman Filter assumptions: [Kalman, 96] Gaussian state PDF p (x k u :k, y :k ) N (ˆx k, ˆP k ) Zero-mean, Gaussian noise PDFs: w k N (, Q k ), n k N (, R k ) 7

8 Background Bayes Filter Infinite Computation Extended Kalman Filter (EKF) [Schmidt, 967] Conventional approach involving linearization Sigma-Point Kalman Filter (SPKF) [Julier et al., 995] Modern approach of passing selective samples through the nonlinearity 8

9 Outline Background Objective Experimental Setup Results Discussion Conclusion 9

10 Objective Literature Review [Julier and Uhlmann, 997] Simulated spacecraft reentry tracking problem Found that the SPKF estimate had a lower mean-squared error compared to ground truth than the EKF [van der Merwe and Wan, 24] Simulated and real GPS/INS UAV guidance problem SPKF had a lower RMS error compared to ground truth than the EKF with similar computational requirements

11 Objective Novel Contribution A comparison of the performance of the EKF and SPKF as approximations to the Bayes Filter, in the context of a real-world nonlinear state estimation problem

12 Objective Methodology Implement algorithms EKF SPKF Bayes Filter (via Monte Carlo Sampling) p(x k u :k, y :k )=ηp(y k x k ) p(x k x k, u k )p(x k u :k, y :k )dx k Real-world nonlinear state estimation problem Indoor mobile rover localization problem 2

13 Outline Background Objective Experimental Setup Results Discussion Conclusion 3

14 Experimental Setup Pioneer3-AT Netbook Vicon markers Laser rangefinder Joystick receiver P3-AT base 4

15 Experimental Setup Vicon System 5

16 The linked image cannot be displayed. The file may have been moved, renamed, or deleted. Verify that the link points to the correct file and location. Experimental Setup Lab Space 6

17 Experimental Setup Video 7

18 The linked image cannot be displayed. The file may have been moved, renamed, or deleted. Verify that the link points to the correct file and location. Experimental Setup Rover Traverse 8

19 Outline Background Objective Experimental Setup Results Discussion Conclusion 9

20 Results Estimation Video 2

21 Outline Background Objective Experimental Setup Results Discussion Conclusion 2

22 Results Error Spike Example EKF SPKF 22

23 Discussion Error Spike Example EKF SPKF 23

24 Discussion Error Spike Example EKF SPKF 24

25 Discussion Recursive Estimation p(ˆx p(x k u :k, y :k )=η p(y k ˆx x k ) p(ˆx p(x k ˆx x k, u k )p(ˆx )p(x k u :k, y :k )dˆx )dx k 25

26 Outline Background Objective Experimental Setup Results Discussion Conclusion 26

27 Conclusion SPKF is easy to implement SPKF outperforms the EKF (in this problem) Handling of nonlinearities Approximating the Bayes Filter However, the Recursive Bayesian approach is fundamentally flawed 27

28 References [Kalman, 96] R. E. Kalman. A new approach to linear filtering and prediction problems. Journal of Basic Engineering, 96. [Schmidt, 966] S. F. Schmidt. Applications of state space methods to navigation problems. In Advanced Control Systems, volume 3, pages Academic Press, 966. [Julier et al., 995] S.J. Julier, J.K. Uhlmann and H.F. Durrant-Whyte. A new approach for filtering nonlinear systems. In the proceedings of the American Control Conference, pages , 995. [Julier and Uhlmann, 997] S. J. Julier and J. K. Uhlmann. A new extension of the Kalman Filter to nonlinear systems. Int. Symp. Aerospace/ Defense Sensing, Simul. and Controls, 997. [van der Merwe and Wan, 24] R. van der Merwe and E. Wan. Sigma-Point Kalman Filters for integrated navigation. In Proceedings of the 6th Annual Meeting of The Institute of Navigation (ION), 24. [Vicon] Vicon MX. 28

29 Questions? 29

30 Algorithms EKF ˆP k = H x,kˆp k H T x,k + H w,k Q k H T w,k ˆx k = h (ˆx k, u k, ) ( K k = ˆP k GT x,k G x,kˆp k GT x,k + G n,k R k G T x,k ˆP k = ( K k G x,k ) ˆP k ˆx k = ˆx k + K ( k yk g (ˆx k, )) ) 3

31 Algorithms SPKF Prediction X i,k := h (X i,k, u k ) ( ˆx k := κx i,k L + κ + 2 ˆP k := L + κ ( κ 2L i= ( X,k ˆx k X i,k ) )( X,k ˆ x k ) T + 2 2L i= ) ( )( ) T X i,k ˆx k X i,k ˆx k 3

32 Algorithms SPKF Correction ( ) Y i,k := g X i,k, N i,k ( ) ŷ k := κy i,k + 2L Y i,k L + κ 2 i= ( V k := κ (Y,k ŷ k )(Y,k ŷ k ) T + L + κ 2 ( ( ) U k := κ X,k L + κ ˆx k (Y,k ŷ k ) T + 2 K k := U k V k ˆx k := ˆx k + K k (y k ŷ k ) ˆP k := ˆP k K ku T k 2L i= 2L i= (Y i,k ŷ k )(Y i,k ŷ k ) T ) ( X i,k ˆx k ) ) (Y i,k ŷ k ) T 32

33 Algorithms Particle Filter (PF) x (m) k p (x k u :k, y :k ) w (m,l m) k p(w k ) ( ) x (m,l m) k := h x (m) k, u k, w (m,l m) l, ( m, l m ) ( )) w (m,l m) k := p y k g (x (m,l m) k, } x (m,l m) k {x (m,l m) k,w (m,l m) k 33

34 Experimental Setup System Models Process Model x k y k θ k } {{ } x k = x k y k θ k }{{} x k +T cos θ k sin θ k sin θ k cos θ k v xk v yk +w k ω k }{{} } {{ } h(x k,u k,w k ) u k w k N (, Q) 34

35 Experimental Setup System Models Process Noise 5 45 Mean:.42, StdDev:.66, Tuned StdDev:.3 fitted Gaussian tuned Gaussian 45 4 Mean:.2, StdDev:.7, Tuned StdDev:.28 fitted Gaussian tuned Gaussian 4 35 Frequency Frequency v x error [m/s] v y error [m/s] 9 8 Mean: 4.4e 8, StdDev:.9, Tuned StdDev:.3 fitted Gaussian tuned Gaussian 7 6 Frequency ω error [rads/s] 35

36 Experimental Setup System Models Measurement Model ] [ rk,l } φ k,l {{ } y k,l = [ ] (x l x k d cos θ k ) 2 +(y l y k d sin θ k ) 2 + n k,l atan2 (y l y k d sin θ k,x l x k d cos θ k ) θ k }{{} g l (x k,n k,l ) n k,l N (, R) 36

37 Experimental Setup System Models Measurement Noise 4 35 Mean:., StdDev:.29, Tuned StdDev:.25 fitted Gaussian tuned Gaussian 35 3 Mean:.42, StdDev:.5, Tuned StdDev:.47 fitted Gaussian tuned Gaussian 3 25 Frequency Frequency r error [m] φ error [rads] 37

38 Results EKF 4 3 EKF estimation error in ˆx estimate error 3σ confidence envelope 5 4 EKF estimation error in ŷ estimate error 3σ confidence envelope ˆx x [m] 2 ŷ y [m] t [s] t [s] 3 2 EKF estimation error in ˆθ estimate error 3σ confidence envelope ˆθ θ [rads] t [s] 38

39 Results SPKF 4 3 SPKF estimation error in ˆx estimate error 3σ confidence envelope 5 4 SPKF estimation error in ŷ estimate error 3σ confidence envelope ˆx x [m] 2 ŷ y [m] t [s] t [s] 3 2 SPKF estimation error in ˆθ estimate error 3σ confidence envelope ˆθ θ [rads] t [s] 39

40 Results PF 4 3 PF- estimation error in ˆx estimate error 3σ confidence envelope 5 4 PF- estimation error in ŷ estimate error 3σ confidence envelope ˆx x [m] 2 ŷ y [m] t [s] t [s] 3 2 PF- estimation error in ˆθ estimate error 3σ confidence envelope ˆθ θ [rads] t [s] 4

41 Results EKF estimation error in x 4 SPKF estimation error in x 4 estimate error 3σ confidence envelope 3 2 x x [m] t [s] EKF estimation error in y SPKF estimation error in y y y [m] 3 2 y y [m] EKF estimation error in θ θ θ [rads] EKF estimate error 3σ confidence envelope 2 t [s] PF- estimation error in θ 3 estimate error 3σ confidence envelope 2 2 t [s] SPKF estimation error in θ 3 θ θ [rads] 8 θ θ [rads] 2 estimate error 3σ confidence envelope t [s] estimate error 3σ confidence envelope t [s] estimate error 3σ confidence envelope PF- estimation error in y 5 estimate error 3σ confidence envelope 2 4 t [s] t [s] 5 4 estimate error 3σ confidence envelope 3 6 y y [m] 3 2 PF- estimation error in x 4 estimate error 3σ confidence envelope 2 x x [m] x x [m] 3 EKF vs. SPKF vs. PF t [s] t [s] SPKF 2 PF 4

42 Results Quantitative Results (all RMS values) EKF SPKF PF ˆx x [m] ŷ y [m] ˆθ θ [rads] ˆx ˆx pf [m].46. N/A ŷ ŷ pf [m].42.2 N/A ˆθ ˆθ pf [rads].5.2 N/A ˆσ x ˆσ xpf [m] N/A ˆσ y ˆσ ypf [m] N/A ˆσ θ ˆσ θpf [rads] N/A 42