Upload
audra-mccormick
View
214
Download
0
Tags:
Embed Size (px)
Citation preview
Slides for the book:Probabilistic Robotics
• Authors:– Sebastian Thrun– Wolfram Burgard– Dieter Fox
• Publisher:– MIT Press, 2005.
• Web site for the book & more slides:http://www.probabilistic-robotics.org/
Reminder: Jacobian Matrix
x=(x1,x2,…,xn)nonlinear
Tangent plane represents multi-dimensional derivative
EKF Linearization is done through First Order Taylor Expansion
Linearization for prediction
Linearization for correction
EKF Linearization: First Order Taylor Series Expansion. Matrix vs Scalar
• Scalar case
• MATRIX case
Linearity Assumption Revisited
Mean of p(x)
Linear mapping of Mean: y=ax+b
Mean of p(y)
Grey represents true distribution of p(y)
This is ideal case of pushing gaussian
through a linear system
Non-linear Function
Gaussian of p(y)
Non-linear function g(x)
Mean of p(x)
This is a real case of pushing a gaussian
through a non-linear system
Grey represents true distribution of p(y)
We are approximating grey by blue, not good
EKF Linearization (1): Taylor approximation and EKF Gaussian
• Better than in last slide.
• The mean is closer to grey shape
This example shows that Gaussian of EKF better represents estimated value than the Gaussian mean
• Even more difference of EKF Gaussian and real Gaussian, but better estimation
EKF Linearization (3)
Smaller max value
Conclusion: NON-GAUSSIAN distribution of output requires EKF or something better that would calculate the mean better
• Gaussian with high value of standard deviation
• In this case the input Gaussian does not create an output Gaussian!
• And Gaussian estimation of output is worse than EKF estimation of Gaussian with respect to mean
Narrow and short gaussian
EKF Linearization (5)Gaussian similar to EKF gaussian
In this case EKF works similarly to KF
Localization of two-dimensional robot
• Given – Map of the environment.– Sequence of sensor measurements.
• Wanted– Estimate of the robot’s position.
• Problem classes– Position tracking– Global localization– Kidnapped robot problem (recovery)
“Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]
1. EKF_localization ( mt-1, St-1, ut, zt, m):
Prediction Phase:
2.
3.
4. ),( 1 ttt ug
Tttt
Ttttt VMVGG 1
,1,1,1
,1,1,1
,1,1,1
1
1
'''
'''
'''
),(
tytxt
tytxt
tytxt
t
ttt
yyy
xxx
x
ugG
tt
tt
tt
t
ttt
v
y
v
y
x
v
x
u
ugV
''
''
''
),( 1
2
43
221
||||0
0||||
tt
ttt
v
vM
Motion noise
Jacobian of g w.r.t location
Predicted mean
Predicted covariance
Jacobian of g w.r.t control
(x,y,)
velocity
Radial velocityVelocity and radial velocity are controls
1. EKF_localization ( mt-1, St-1, ut, zt, m):
Correction phase:
2.
3.
4.
5.
6.
)ˆ( ttttt zzK
tttt HKI
,
,
,
,
,
,),(
t
t
t
t
yt
t
yt
t
xt
t
xt
t
t
tt
rrr
x
mhH
,,,
2,
2,
,2atanˆ
txtxyty
ytyxtxt
mm
mmz
tTtttt QHHS 1 t
Tttt SHK
2
2
0
0
r
rtQ
Predicted measurement mean
Pred. measurement covariance
Kalman gain
Updated mean
Updated covariance
Jacobian of h w.r.t location
r = range = angle
EKF Prediction Step for four cases
Updated mean
Updated covariance
),( 1 ttt ug Tttt
Ttttt VMVGG 1
Predicted mean
Predicted covariance
32
EKF Prediction Step for four cases: now we take other system
Updated mean
Updated covariance
),( 1 ttt ug
Tttt
Ttttt VMVGG 1
Predicted mean
Predicted covariance
33
EKF Observation Prediction Step
Z = measurement
robot
landmark
Predicted robot covariance
Similarly for three other cases but sizes and shapes are different
Estimation Sequence (1)
landmarkInitial true robot Initial
estimated robot
Based on measurement only Based on Kalman
observe that now our measurements are less precise
Estimation Sequence (2)
And we see a difference between estimated and real positions of the robot
38
EKF Summary• Highly efficient: Polynomial in measurement
dimensionality k and state dimensionality n: O(k2.376 + n2)
• Not optimal!• Can diverge if nonlinearities are large!• Works surprisingly well even when all
assumptions are violated!
State Representation for EKF SLAM
Mean Value Matrix Covariance matrix
Please observe how these matrices are partitioned to submatrices
EKF-SLAM: a concrete example of robot on a plane
• Robot moves in the plane• Velocity-based motion model:
1. V = speed2. = radial velocity
• Robot observes point landmarks• Range-bearing sensor • Known data association to landmark points• Known number of landmarks.