7-1 Probabilistic Robotics: Kalman Filters Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian Plagemann, Dirk Haehnel, Mike Montemerlo, Nick Roy, Kai Arras, Patrick Pfaff and others Sebastian Thrun & Alex Teichman Stanford Artificial Intelligence Lab
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
7-1
Probabilistic Robotics: Kalman Filters
Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian Plagemann, Dirk Haehnel, Mike Montemerlo, Nick Roy, Kai Arras, Patrick Pfaff and others
Sebastian Thrun & Alex TeichmanStanford Artificial Intelligence Lab
7-2
Bayes Filters in Localization
111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel
• Prediction
• Measurement Update
Bayes Filter Reminder
111 )(),|()( tttttt dxxbelxuxpxbel
)()|()( tttt xbelxzpxbel
Gaussians
2
2)(21
2
21)(
:),(~)(
x
exp
Nxp
-
Univariate
)()(21
2/12/
1
)2(1)(
:)(~)(
μxΣμx
Σx
Σμx
t
ep
,Νp
d
Multivariate
),(~),(~ 22
2
abaNYbaXY
NX
Properties of Gaussians
22
21
222
21
21
122
21
22
212222
2111 1,~)()(
),(~),(~
NXpXp
NXNX
• We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations.
),(~),(~ TAABANY
BAXYNX
Multivariate Gaussians
€
X1 ~ N(μ1,Σ1)X2 ~ N(μ2,Σ2)
⎫ ⎬ ⎭⇒ p(X1) ⋅ p(X2) ~ N Σ1
−1
Σ1−1 + Σ2
−1 μ1 + Σ2−1
Σ1−1 + Σ2
−1 μ2,1
Σ1−1 + Σ2
−1
⎛ ⎝ ⎜
⎞ ⎠ ⎟
7-7
Discrete Kalman Filter
tttttt uBxAx 1
tttt xCz
Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation
with a measurement
7-8
Components of a Kalman Filter
t
Matrix (nxn) that describes how the state evolves from t to t-1 without controls or noise.
tA
Matrix (nxl) that describes how the control ut changes the state from t to t-1.tB
Matrix (kxn) that describes how to map the state xt to an observation zt.tC
t
Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Rt and Qt respectively.
7-9
Kalman Filter Updates in 1D
7-10
Kalman Filter Updates in 1D
1)(with )(
)()(
tTttt
Tttt
tttt
ttttttt QCCCK
CKICzK
xbel
2,
2
2
22 with )1(
)()(
tobst
tt
ttt
tttttt K
KzK
xbel
Kalman Filter Updates in 1D
tTtttt
tttttt RAA
uBAxbel
1
1)(
2
,2221)(
tactttt
tttttt a
ubaxbel
Kalman Filter Updates
0000 ,;)( xNxbel
Linear Gaussian Systems: Initialization
• Initial belief is normally distributed:
• Dynamics are linear function of state and control plus additive noise:
tttttt uBxAx 1
Linear Gaussian Systems: Dynamics
ttttttttt RuBxAxNxuxp ,;),|( 11
1111
111
,;~,;~
)(),|()(
ttttttttt
tttttt
xNRuBxAxN
dxxbelxuxpxbel
Linear Gaussian Systems: Dynamics
tTtttt
tttttt
ttttT
tt
ttttttT
tttttt
ttttttttt
tttttt
RAAuBA
xbel
dxxx
uBxAxRuBxAxxbel
xNRuBxAxN
dxxbelxuxpxbel
1
1
1111111
11
1
1111
111
)(
)()(21exp
)()(21exp)(
,;~,;~
)(),|()(
• Observations are linear function of state plus additive noise:
• 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]