Robot Localization and Kalman Filters

Transcription

1 Robot Localization and Kalman Filters Rudy Negenborn August 26, 2003

2 Outline Robot Localization Probabilistic Localization Kalman Filters Kalman Localization Kalman Localization with Landmarks 2

3 Robot Localization Localization a key problem Available location information Relative Measurements Driving: wheel encoders, accerelometers,, gyroscopes Frequent,, but increasing error Absolute Measurements Sensing: : GPS, vision, laser, landmarks Less frequent,, but bounded error 3

4 Probabilistic Localization Probabilistic approach Consider whole space of locations Belief Bel( x k ) = P( x k d 1,..., d k ) Get belief as close to real distribution as possible Prior Belief Bel - ( x k ) = P( x k z 1, a 1,..., z k-1, a k-1 ) Posterior Belief Bel + ( x k ) = P( x k z 1, a 1,..., z k-1, a k-1, z k ) 4

5 Probabilistic Localization Localization equations: Bel - ( x k ) = P( x k z 1, a 1,..., z k-1, a k-1 ) = P( x k a k-1, x k-1 ) Bel + ( x k-1 ) dx k-1 Bel + ( x k ) = P( x k z 1, a 1,, z k-1, a k-1, z k ) = P( z k x k ) Bel - ( x k ) P( z k z 1, a 1,, z k-1, a k-1 ) Markov Assumption Implementation Issues: Motion model: P( x k a Measurement model: P( Representation of belief a k-1, x k-1 ) P( z k x k ) 5

6 Kalman Filters Representation of belief Gaussian function Mean and (co)variance( Initial belief: Bel( ( x 0 ) = N( x 0, P 0 ) Motion model x k = Ax k-1 + Ba k-1 + w k-1, where w k N( 0, Q k ) P( x k a k-1, x k-1 ) = N( Ax k-1, Q k ) Measurement model z k = Hx k + v k, where v k N( 0, R k ) P( z k x k ) = N( Hx k, R k ) 6

7 Kalman Filters Representation of belief Gaussian function Mean and (co)variance( Initial belief: Bel( ( x 0 ) = N( x 0, P 0 ) Motion model x k = Ax k-1 + Ba k-1 + w k-1, where w k N( 0, Q k ) P( x k a k-1, x k-1 ) = N( AxA k-1, Q k ) Measurement model z k = Hx k + v k, where v k N( 0, R k ) P( z k x k ) = N( Hx k, R k ) 7

8 Kalman Filters Prior belief: Bel - ( x k ) = N( ^x - k, Pk - ) Prior localization estimate: Prior uncertainty: Posterior belief: : Bel + ( x k ) = N( ^x + k, Pk + ) Posterior localization estimate: Posterior uncertainty: 8

9 Kalman Filters Prior belief: Bel - ( x k ) = N( ^x - k, Pk - ) Prior location estimate: Prior uncertainty: P- k Posterior belief: : Bel + ( x k ) = N( ^x + k, Pk + ) ^x k - Posterior location estimate: Posterior uncertainty: P+ k ^x k + 9

10 Kalman Filters Prior belief: Bel - ( x k ) = N( ^x - k, Pk - ) ^x - k = A A ^x + k-1 + B B ^a k-1 P- = A A + A T + B B B T k + Q k-1 P k-1 U k-1 10

11 Kalman Filters Prior belief: Bel - ( x k ) = N( ^x - k, Pk - ) ^x - k = A A ^x + k-1 + B B ^a k-1 Prior location estimate Posterior location estimate Last relative measurement P- = A A + A T + B B B T k + Q k-1 P k-1 U k-1 11

12 Kalman Filters Prior belief: Bel - ( x k ) = N( ^x - k, Pk - ) ^x - k = A A ^x + k-1 + B B ^a k-1 Prior location estimate Posterior location estimate Last relative measurement P- = A A + A T + B B B T k + Q k-1 Prior uncertainty P k-1 Posterior uncertainty U k-1 Relative measurement uncertainty Motion uncertainty 12

13 Kalman Filters Posterior belief: Bel + ( x k ) = N( ^x + k, Pk + ) ^x + k = ^x - k + K k ( z k H ^x - k ) K k = P- HT ( H P- H T + R k ) 1 k k P+ k = ( I K k H ) P- k 13

14 Kalman Filters Posterior belief: Bel + ( x k ) = N( ^x + k, Pk + ) ^x + k = ^x - k + K k ( z k H ^x - k ) Residual Posterior state estimate Prior state estimate Kalman Gain True measurement Measurement prediction K k = P- HT ( H P- H T + R k ) 1 k k P+ k = ( I K k H ) P- k 14

15 Kalman Filters Posterior belief: Bel + ( x k ) = N( ^x + k, Pk + ) ^x + k = ^x - k + K k ( z k H ^x - k ) Residual Posterior state estimate Prior state estimate Kalman Gain True measurement Measurement prediction K k = P- HT ( H P- H T + R k ) 1 k k P+ k = ( I K k H ) P- k Measurement residual uncertainty 15

16 Kalman Gain K measurement noise prior state uncertainty 16

17 Kalman Filters Posterior belief: Bel + ( x k ) = N( ^x + k, Pk + ) ^x + k = ^x - k + K k ( z k H ^x - k ) Residual Posterior state estimate Prior state estimat Kalman Gain True measurement Measurement prediction K k = P- HT ( H P- H T + R k ) 1 k k P+ k = ( I K k H ) P- k Measurement residual uncertainty 17

18 Extended Kalman Filter Nonlinear motion and measurement models Linearization around estimated trajectory Partial derivatives of nonlinear model for A, B, H Close to linear over uncertainty region Drawbacks Evaluation at every time step Linearization errors 18

19 Kalman Localization Localization instances Position Tracking Initial belief with peak at true initial location Global Localization Initial uniform belief Kidnapped Robot Initial belief with peak far from true location 19

20 Position Tracking y S x 20

21 Position Tracking value step value step value step 21

22 Position Tracking y x 22

23 Infrequent Measurements 20 prior and posterior uncertainty in state element 1 (co)variance prior and posterior uncertainty step in state element (co)variance prior and posterior uncertainty step in state element (co)variance step 23

24 Kalman Localization with Landmarks Uniquely identifiable landmarks 1:1 correspondence Type identifiable landmarks 1:n correspondence Kalman Filter framework extension Multiple state beliefs Probability for each belief 24

25 Type Identifiable Landmarks true x,y trajectory with measurements lm=1 lm=2 lm=3 lm=4 lm= y(cm) S x(cm) 25

26 Type Identifiable Landmarks x

27 Type Identifiable Landmarks

28 Type Identifiable Landmarks

29 Type Identifiable Landmarks 110 true x,y trajectory with measurements y(cm) x(cm) 29

30 Summary & Future Summary Describing theory of localization and Kalman Filters Illustrating applications of Kalman Filter to localization problems Extension of Kalman Filter framework to multiple beliefs Future work Practical application to robots Possibilities of Kalman Filter extension Website:

31 The end. 31