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
rudy@negenborn.net rudy@negenborn.net August 26, 2003 August 26, 2003
26-9-2003 2 Robot Localization and Kalman Filters
26-9-2003 3 Robot Localization and Kalman Filters
Driving: : wheel wheel encoders encoders, , accerelometers accerelometers, gyroscopes , gyroscopes
Frequent, but , but increasing increasing error error
Sensing: GPS, vision, laser, landmarks : GPS, vision, laser, landmarks
Less frequent frequent, but , but bounded bounded error error
26-9-2003 4 Robot Localization and Kalman Filters
Consider whole whole space space of
locations
Bel( x xk
k ) = P(
) = P( x xk
k | d
| d1
1, ... ,
, ... , d dk
k )
)
Get belief belief as as close close to real distribution as possible to real distribution as possible
Prior Belief Belief
Bel-
( x xk
k ) = P(
) = P( x xk
k | z
| z1
1, a
, a1
1, ... , z
, ... , zk
k-
1, a
, ak
k-
1 )
)
Posterior Belief Belief
Bel+
( x xk
k ) = P(
) = P( x xk
k | z
| z1
1, a
, a1
1, ... , z
, ... , zk
k-
1, a
, ak
k-
1,
, z zk
k )
)
26-9-2003 5 Robot Localization and Kalman Filters
Bel-
( x xk
k )
) = = P( P( x xk
k | z
| z1
1, a
, a1
1, ..., z
, ..., zk
k-
1, a
, ak
k-
1 )
) = = ∫ ∫ P( P( x xk
k | a
| ak
k-
1, x
, xk
k-
1 )
)· · Bel Bel+
+( x
( xk
k-
1 ) dx
) dxk
k-
1
Bel+
+(
( x xk
k )
) = = P( P( x xk
k | z
| z1
1, a
, a1
1, …, z
, …, zk
k-
1, a
, ak
k-
1,
, z zk
k )
)
Motion model: P(
P( x xk
k | a
| ak
k-
1, x
, xk
k-
1 )
)
Measurement model: model: P(
P( z zk
k |
| x xk
k )
)
Representation of
belief
P( P( z zk
k |
| x xk
k )
) Bel Bel-
( x xk
k )
) P( P( z zk
k | z
| z1
1, a
, a1
1, …, z
, …, zk
k-
1, a
, ak
k-
1 )
) =
Markov Assumption
26-9-2003 6 Robot Localization and Kalman Filters
Gaussian function
Mean and (co)variance co)variance
Initial belief: Bel Bel( x ( x0
0 ) = N( x
) = N( x0
0, P
, P0
0 )
)
xk
k = Ax
= Axk
k-
1 + Ba
+ Bak
k-
1 + w
+ wk
k-
1, where w
, where wk
k ∼
∼ N( 0, N( 0, Q Qk
k )
) P( xk |ak-1, xk-1 ) = N( Axk-1, Qk )
zk
k =
= Hx Hxk
k +
+ v vk
k, where
, where v vk
k ∼
∼ N( 0, N( 0, R Rk
k )
) P( zk|xk ) = N( Hxk, Rk )
26-9-2003 7 Robot Localization and Kalman Filters
Gaussian function
Mean and (co)variance co)variance
Initial belief: Bel Bel( x ( x0
0 ) = N( x
) = N( x0
0, P
, P0
0 )
)
xk
k = Ax
= Axk
k-
1 + Ba
+ Bak
k-
1 + w
+ wk
k-
1, where w
, where wk
k ∼
∼ N( 0, N( 0, Q Qk
k )
)
P( x xk
k |a
|ak
k-
1, x
, xk
k-
1 ) = N( A
) = N( Ax xk
k-
1,
, Q Qk
k )
)
zk
k =
= Hx Hxk
k +
+ v vk
k, where
, where v vk
k ∼
∼ N( 0, N( 0, R Rk
k )
)
P( z zk
k|x
|xk
k ) = N(
) = N( Hx Hxk
k, , R
Rk
k )
)
26-9-2003 8 Robot Localization and Kalman Filters
k ) = N( , )
Prior localization estimate: Prior uncertainty: :
k ) = N( , )
Posterior localization estimate: Posterior uncertainty:
k
k
k
k
26-9-2003 9 Robot Localization and Kalman Filters
k ) = N( , )
Prior location estimate estimate: :
Prior uncertainty uncertainty: :
k ) = N( , )
Posterior location estimate:
Posterior uncertainty:
k
k
^ x xk
k+
+ P Pk
k
+ + ^ ^ x xk
k-
Pk
k
k
k
26-9-2003 10 Robot Localization and Kalman Filters
Prior belief: Bel Bel-
( x xk
k ) = N( , )
) = N( , )
= A· · + B + B· ·
= A· · · · A AT
T + B
+ B· · · · B BT
T + Q
+ Qk
k-
1
^ ^ x xk
k-
Pk
k
^ x xk
k-
^ x xk
k-
1
+ + ^ ^ a ak
k-
1
P Pk
k
Pk
k-
1
+ + U Uk
k-
1
26-9-2003 11 Robot Localization and Kalman Filters
Prior belief: Bel Bel-
( x xk
k ) = N( , )
) = N( , )
= A· · + B + B· ·
= A· · · · A AT
T + B
+ B· · · · B BT
T + Q
+ Qk
k-
1
^ ^ x xk
k-
Pk
k
^ x xk
k-
^ x xk
k-
1
+ + ^ ^ a ak
k-
1
P Pk
k
Pk
k-
1
+ +
Last relative measurement Prior location estimate Posterior location estimate
U Uk
k-
1
26-9-2003 12 Robot Localization and Kalman Filters
Prior belief: Bel Bel-
( x xk
k ) = N( , )
) = N( , )
= A· · + B + B· ·
= A· · · · A AT
T + B
+ B· · · · B BT
T + Q
+ Qk
k-
1
^ ^ x xk
k-
Pk
k
^ x xk
k-
^ x xk
k-
1
+ + ^ ^ a ak
k-
1
P Pk
k
Pk
k-
1
+ +
Last relative measurement Prior location estimate Prior uncertainty Posterior uncertainty Relative measurement uncertainty Motion uncertainty Posterior location estimate
U Uk
k-
1
26-9-2003 13 Robot Localization and Kalman Filters
Posterior belief: Bel Bel+
( x xk
k ) = N( , )
) = N( , )
= + K Kk
k·
· ( ( z zk
k − H
− H· ) · )
Kk
k =
= · · H HT
T·
· ( H ( H· · H · · HT
T +
+ R Rk
k )
)−1
−1
= ( I − K Kk
k· H )·
· H )· ^ ^ x xk
k+
+ P Pk
k
+ + ^ ^ x xk
k+
+ ^ ^ x xk
k-
^ x xk
k-
Pk
k
Pk
k
Pk
k
+ + P Pk
k
26-9-2003 14 Robot Localization and Kalman Filters
Posterior belief: Bel Bel+
( x xk
k ) = N( , )
) = N( , )
= + K Kk
k·
· ( ( z zk
k − H
− H· ) · )
Kk
k =
= · · H HT
T·
· ( H ( H· · H · · HT
T +
+ R Rk
k )
)−1
−1
= ( I − K Kk
k· H )·
· H )· ^ ^ x xk
k+
+ P Pk
k
+ + ^ ^ x xk
k+
+ ^ ^ x xk
k-
^ x xk
k-
Pk
k
Pk
k
Pk
k
+ + P Pk
k
Prior state estimate Kalman Gain True measurement Measurement prediction Posterior state estimate
26-9-2003 15 Robot Localization and Kalman Filters
Posterior belief: Bel Bel+
( x xk
k ) = N( , )
) = N( , )
= + K Kk
k·
· ( ( z zk
k − H
− H· ) · )
Kk
k =
= · · H HT
T·
· ( H ( H· · H · · HT
T +
+ R Rk
k )
)−1
−1
= ( I − K Kk
k· H )·
· H )· ^ ^ x xk
k+
+ P Pk
k
+ + ^ ^ x xk
k+
+ ^ ^ x xk
k-
^ x xk
k-
Pk
k
Pk
k
Pk
k
+ + P Pk
k
Prior state estimate Kalman Gain True measurement Measurement prediction Posterior state estimate Measurement residual uncertainty
26-9-2003 16 Robot Localization and Kalman Filters
5 10 15 20 25 5 10 15 20 25 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 prior state uncertainty measurement noise K
26-9-2003 17 Robot Localization and Kalman Filters
Posterior belief: Bel Bel+
( x xk
k ) = N( , )
) = N( , )
= + K Kk
k·
· ( ( z zk
k − H
− H· ) · )
Kk
k =
= · · H HT
T·
· ( H ( H· · H · · HT
T +
+ R Rk
k )
)−1
−1
= ( I − K Kk
k· H )·
· H )· ^ ^ x xk
k+
+ P Pk
k
+ + ^ ^ x xk
k+
+ ^ ^ x xk
k-
^ x xk
k-
Pk
k
Pk
k
Pk
k
+ + P Pk
k
Prior state estimat Kalman Gain True measurement Measurement prediction Posterior state estimate Measurement residual uncertainty
26-9-2003 18 Robot Localization and Kalman Filters
Partial derivatives of nonlinear model for A, B, H
Close to linear over uncertainty region
Evaluation at every time step
Linearization errors
26-9-2003 19 Robot Localization and Kalman Filters
Initial belief with peak at true initial location
Initial uniform belief
Initial belief with peak far from true location
26-9-2003 20 Robot Localization and Kalman Filters
−100 100 200 300 400 500 600 700 800 −200 −100 100 200 300 400 500 600 700 x y S
26-9-2003 21 Robot Localization and Kalman Filters
500 1000 1500 2000 2500 3000 −400 −200 200 400 step value 500 1000 1500 2000 2500 3000 −400 −200 200 400 step value 500 1000 1500 2000 2500 3000 −2 −1 1 2 step value
26-9-2003 22 Robot Localization and Kalman Filters
100 150 200 250 300 350 88 90 92 94 96 98 100 102 x y
←1 ←1 ←1 ←2 ←2 ←2 ←3 ←3 ←3 ←4 ←4 ←4 ←5 ←5 ←5 ←6 ←6 ←6 ←7 ←7 ←7 ←8 ←8 ←8 ←9 ←9 ←9 ←10 ←10 ←10 ←10 ←10 ←11 ←11 ←11 ←12 ←12 ←12 ←13 ←13 ←13 ←14 ←14 ←14 ←15 ←15 ←15 ←16 ←16 ←16 ←17 ←17 ←17 ←18 ←18 ←18 ←19 ←19 ←19 ←20 ←20 ←20 ←20 ←20
26-9-2003 23 Robot Localization and Kalman Filters
10 20 30 40 50 60 70 80 90 100 5 10 15 20 step (co)variance prior and posterior uncertainty in state element 1 10 20 30 40 50 60 70 80 90 100 50 100 150 step (co)variance prior and posterior uncertainty in state element 5 10 20 30 40 50 60 70 80 90 100 0.005 0.01 0.015 step (co)variance prior and posterior uncertainty in state element 9
26-9-2003 24 Robot Localization and Kalman Filters
Multiple state beliefs
Probability for each belief
26-9-2003 25 Robot Localization and Kalman Filters
200 400 600 800 1000 1200 1400 1600 1800 2000 100 120 140 160 180 200 220 240 x(cm) y(cm) true x,y trajectory with measurements
S
26-9-2003 26 Robot Localization and Kalman Filters
500 1000 1500 2000 80 90 100 110 120 130 1 2 3 4 5 6 x 10
−3
26-9-2003 27 Robot Localization and Kalman Filters
500 1000 1500 2000 80 90 100 110 120 130 0.002 0.004 0.006 0.008 0.01
26-9-2003 28 Robot Localization and Kalman Filters
500 1000 1500 2000 80 90 100 110 120 130 0.005 0.01 0.015 0.02 0.025
26-9-2003 29 Robot Localization and Kalman Filters
200 400 600 800 1000 1200 1400 1600 1800 2000 90 95 100 105 110 x(cm) y(cm) true x,y trajectory with measurements 1 2 3 4 5
26-9-2003 30 Robot Localization and Kalman Filters
Summary
Describing theory of localization and Kalman Kalman Filters Filters
Illustrating applications of Kalman Kalman Filter to localization problems Filter to localization problems
Extension of Kalman Kalman Filter framework to multiple beliefs Filter framework to multiple beliefs
Future work
Practical application to robots
Possibilities of Kalman Kalman Filter extension Filter extension
Website: http:// http://www.negenborn.net/kal_loc www.negenborn.net/kal_loc/ /
26-9-2003 31 Robot Localization and Kalman Filters