Robot Localization Localization Robot and and Kalman Filters - PowerPoint PPT Presentation

Robot Localization Localization Robot and and Kalman Filters Filters Kalman Rudy Negenborn Negenborn Rudy rudy@negenborn.net rudy@negenborn.net August 26, 2003 August 26, 2003

Outline Outline � Robot Robot Localization Localization � � Probabilistic Probabilistic Localization Localization � � Kalman Kalman Filters Filters � � Kalman Kalman Localization Localization � � Kalman Kalman Localization Localization with with Landmarks Landmarks � Robot Localization and Kalman Filters 2 26-9-2003

Robot Localization Robot Localization � Localization Localization a a key key problem problem � � Available Available location information location information � � Relative Relative Measurements Measurements � � Driving Driving: : wheel wheel encoders encoders, , accerelometers accerelometers, gyroscopes , gyroscopes � � Frequent Frequent, but , but increasing increasing error error � � Absolute Absolute Measurements Measurements � � Sensing Sensing: GPS, vision, laser, landmarks : GPS, vision, laser, landmarks � � Less Less frequent frequent, but , but bounded bounded error error � Robot Localization and Kalman Filters 3 26-9-2003

Probabilistic Localization Localization Probabilistic � Probabilistic Probabilistic approach approach � � Consider Consider whole whole space space of of locations locations � � Belief Belief � � Bel( Bel( x x k ) = P( x x k | d 1 , ... , d d k ) k ) = P( k | d 1 , ... , k ) � � Get Get belief belief as as close close to real distribution as possible to real distribution as possible � � Prior Prior Belief Belief � - ( Bel - � Bel ( x x k ) = P( x x k | z 1 , a 1 , ... , z k , a k ) k ) = P( k | z 1 , a 1 , ... , z 1 , a 1 ) � k- -1 k- -1 � Posterior Posterior Belief Belief � + ( Bel + � Bel ( x x k ) = P( x x k | z 1 , a 1 , ... , z k , a k , z z k ) k ) = P( k | z 1 , a 1 , ... , z 1 , a 1 , k ) � k- -1 k- -1 Robot Localization and Kalman Filters 4 26-9-2003

Probabilistic Localization Localization Probabilistic � Localization Localization equations equations: : � � Bel Bel - - ( ( x x k ) = P( P( x x k | z 1 , a 1 , ..., z k , a k ) k ) = k | z 1 , a 1 , ..., z 1 , a 1 ) � k- -1 k- -1 Markov ∫ P( = ∫ P( x x k | a k , x k )· · Bel Bel + + ( x ( x k ) dx k = k | a 1 , x 1 ) 1 ) dx k- -1 k- -1 k- -1 k- -1 1 Assumption � Bel Bel + + ( ( x x k ) = P( P( x x k | z 1 , a 1 , …, z k , a k , z z k ) k ) = k | z 1 , a 1 , …, z 1 , a 1 , k ) � k- -1 k- -1 P( z P( z k k | | x x k k ) ) Bel Bel - - ( ( x x k k ) ) = P( z z k | z 1 , a 1 , …, z k , a k ) P( k | z 1 , a 1 , …, z 1 , a 1 ) k- -1 k- -1 � Implementation Implementation Issues Issues: : � � Motion model: Motion model: P( P( x x k | a k , x k ) k | a 1 , x 1 ) � k- -1 k- -1 � Measurement Measurement model: model: P( P( z z k | x x k ) k | k ) � � Representation Representation of of belief belief � Robot Localization and Kalman Filters 5 26-9-2003

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

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

Kalman Filters Filters Kalman - ( Bel - - - P - - ^ ^ � Prior belief: Prior belief: Bel ( x x k ) = N( , ) x k k ) = N( , ) P k x � k k Prior localization estimate: Prior uncertainty: : + ( : Bel + + + P + + ^ ^ � Posterior Posterior belief belief: Bel ( x x k ) = N( , ) x k k ) = N( , ) P k x � k k Posterior localization estimate: Posterior uncertainty: Robot Localization and Kalman Filters 8 26-9-2003

Kalman Filters Filters Kalman - ( Bel - - - P - - ^ ^ � Prior belief: Prior belief: Bel ( x x k ) = N( , ) x k k ) = N( , ) P k x � k k - k - ^ ^ � Prior location Prior location estimate estimate: : x k x � - - P k � Prior Prior uncertainty uncertainty: : P � k + ( : Bel + + + P + + ^ ^ � Posterior Posterior belief belief: Bel ( x x k ) = N( , ) x k k ) = N( , ) P k x � k k + k + ^ ^ x k � Posterior location estimate: Posterior location estimate: x � + + � Posterior uncertainty: Posterior uncertainty: P k P � k Robot Localization and Kalman Filters 9 26-9-2003

Kalman Filters Filters Kalman - ( Bel - - P - k - - ^ ^ x k � Prior belief: Prior belief: Bel ( x x k ) = N( , ) x P k k ) = N( , ) � k - + k - + ^ ^ ^ ^ ^ ^ x k = A· · x k + B· · x = A x + B a k a � � k- -1 1 k- -1 1 - + - + T + B T + Q = A· · · A A T + B· · · B B T + Q k = A · · P k P P k U k U P � � k- -1 1 k k- -1 1 k- -1 1 Robot Localization and Kalman Filters 10 26-9-2003

Kalman Filters Filters Kalman - ( Bel - - P - k - - ^ ^ x k � Prior belief: Prior belief: Bel ( x x k ) = N( , ) x P k k ) = N( , ) � k - + k - + ^ ^ ^ ^ ^ ^ x k = A· · x k + B· · x = A x + B a k a � � k- -1 1 k- -1 1 Last relative Prior location Posterior measurement estimate location estimate - + - + T + B T + Q = A· · · A A T + B· · · B B T + Q k = A · · P k P P k U k U P � � k- -1 1 k k- -1 1 k- -1 1 Robot Localization and Kalman Filters 11 26-9-2003

Kalman Filters Filters Kalman - ( Bel - - P - k - - ^ ^ x k � Prior belief: Prior belief: Bel ( x x k ) = N( , ) x P k k ) = N( , ) � k - + k - + ^ ^ ^ ^ ^ ^ x k = A· · x k + B· · x = A x + B a k a � � k- -1 1 k- -1 1 Last relative Prior location Posterior measurement estimate location estimate - + - + T + B T + Q = A· · · A A T + B· · · B B T + Q k = A · · P k P P k U k U P � � k- -1 1 k k- -1 1 k- -1 1 Motion uncertainty Relative Posterior Prior measurement uncertainty uncertainty uncertainty Robot Localization and Kalman Filters 12 26-9-2003

Kalman Filters Filters Kalman + ( Bel + + P + k + + ^ ^ � Posterior belief: Posterior belief: Bel ( x x k ) = N( , ) x k k ) = N( , ) x P k � k - - + k - k - k + ^ ^ ^ ^ ^ ^ x k x k x k = + K = + x K k k · · ( ( z z k k − H − H· ) · ) x x � � - - - - T + � K K k = · P k · H H T T · · ( H ( H· · H · · H T P k + R R k ) − 1 − 1 k = P P k ) � k k + - + - P k = ( I − K K k · H )· P k P = ( I − k · H )· P � � k k Robot Localization and Kalman Filters 13 26-9-2003

Kalman Filters Filters Kalman + ( Bel + + P + k + + ^ ^ � Posterior belief: Posterior belief: Bel ( x x k ) = N( , ) x k k ) = N( , ) x P k � k Residual - - + k - k - k + ^ ^ ^ ^ ^ ^ x k x k x k = + K = + x K k k · · ( ( z z k k − H − H· ) · ) x x � � Kalman Posterior state Measurement Prior state True Gain estimate prediction estimate measurement - - - - T + � K K k = · P k · H H T T · · ( H ( H· · H · · H T P k + R R k ) − 1 − 1 k = P P k ) � k k + - + - P k = ( I − K K k · H )· P k P = ( I − k · H )· P � � k k Robot Localization and Kalman Filters 14 26-9-2003