ee565:mobile roboticsweb.lums.edu.pk/~akn/files/other/teaching/mobile... · ee565: mobile robotics...
TRANSCRIPT
EE565:Mobile Robotics
Welcome
Dr. Ahmad Kamal Nasir
16.02.2015 Dr. Ahmad Kamal Nasir 1
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
Today’s Objectives
• Recursive State Estimation: Bayes Filter
• Linear Kalman Filter
• Extended Kalman Filter
16.02.2015 Dr. Ahmad Kamal Nasir 2
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
3
Bayes Formula
evidence
prior likelihood
)(
)()|()(
)()|()()|(),(
yP
xPxyPyxP
xPxyPyPyxPyxP
16.02.2015 Dr. Ahmad Kamal Nasir
)()|(
1)(
)()|()(
)()|()(
1
xPxyPyP
xPxyPyP
xPxyPyxP
x
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
4
Simple Example of State Estimation
• Suppose a robot obtains measurement z
• What is P(open|z)?
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
5
Example • P(z|open) = 0.6 P(z|open) = 0.3
• P(open) = P(open) = 0.5
67.03
2
5.03.05.06.0
5.06.0)|(
)()|()()|(
)()|()|(
zopenP
openpopenzPopenpopenzP
openPopenzPzopenP
• z raises the probability that the door is open.
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
• Prediction
• Correction
Bayes Filter
111 )(),|()( tttttt dxxbelxuxpxbel
)()|()( tttt xbelxzpxbel
16.02.2015 Dr. Ahmad Kamal Nasir 6
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
Kalman Filter
• Bayes filter with Gaussians
• Developed in the late 1950's
• Most relevant Bayes filter variant in practice
• Applications range from economics, weather forecasting, satellite navigation to robotics and many more.
• The Kalman filter “algorithm” is a couple of matrix multiplications!
16.02.2015 Dr. Ahmad Kamal Nasir 7
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
Gaussians
2
2)(
2
1
2
2
1)(
:),(~)(
x
exp
Nxp
-
Univariate
)()(2
1
2/12/
1
)2(
1)(
:)(~)(
μxΣμx
Σx
Σμx
t
ep
,Νp
d
Multivariate
16.02.2015 Dr. Ahmad Kamal Nasir 8
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
Gaussians
16.02.2015 Dr. Ahmad Kamal Nasir 9
1D
2D
3D
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
),(~),(~ 22
2
abaNYbaXY
NX
Properties of Gaussians
2
2
2
1
22
2
2
1
2
112
2
2
1
2
2212
222
2
111 1,~)()(
),(~
),(~
NXpXp
NX
NX
16.02.2015 Dr. Ahmad Kamal Nasir 10
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
• We stay Gaussian as long as we start with Gaussians and
perform only linear transformations.
),(~),(~
TAABANYBAXY
NX
Multivariate Gaussians
1
2
1
1
2
21
11
21
221
222
111 1,~)()(
),(~
),(~
NXpXp
NX
NX
16.02.2015 Dr. Ahmad Kamal Nasir 11
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
12
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
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
13
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.
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
14
Kalman Filter Updates in 1D
t
T
tttt
ttttt
tRAA
uBAxbel
1
1)(
2
,
222
1)(
tactttt
ttttt
ta
ubaxbel
16.02.2015 Dr. Ahmad Kamal Nasir
How to get the magenta one? State prediction step
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
15
Kalman Filter Updates in 1D
1)(with )(
)()(
t
T
ttt
T
ttttttt
tttttt
t QCCCKCKI
CzKxbel
2
,
2
2
22with
)1(
)()(
tobst
tt
ttt
ttttt
t KK
zKxbel
16.02.2015 Dr. Ahmad Kamal Nasir
How to get the blue one? Kalman correction step
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
16
Kalman Filter Updates
16.02.2015 Dr. Ahmad Kamal Nasir
Prediction
Measurements Correction
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
• Prediction
• Correction
Bayes Filter: Reminder
111 )(),|()( tttttt dxxbelxuxpxbel
)()|()( tttt xbelxzpxbel
16.02.2015 Dr. Ahmad Kamal Nasir 17
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
18
0000 ,;)( xNxbel
Linear Gaussian Systems: Initialization
• Initial belief is normally distributed:
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
19
• 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
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
20
Linear Gaussian Systems: Dynamics
t
T
tttt
ttttt
t
tttt
T
tt
tttttt
T
tttttt
ttttttttt
tttttt
RAA
uBAxbel
dxxx
uBxAxRuBxAxxbel
xNRuBxAxN
dxxbelxuxpxbel
1
1
111
1
111
1
1
1
1111
111
)(
)()(2
1exp
)()(2
1exp)(
,;~,;~
)(),|()(
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
21
• Observations are linear function of state plus additive noise:
tttt xCz
Linear Gaussian Systems: Observations
tttttt QxCzNxzp ,;)|(
ttttttt
tttt
xNQxCzN
xbelxzpxbel
,;~,;~
)()|()(
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
22
Linear Gaussian Systems: Observations
1
11
)(with )(
)()(
)()(2
1exp)()(
2
1exp)(
,;~,;~
)()|()(
t
T
ttt
T
ttttttt
tttttt
t
ttt
T
tttttt
T
tttt
ttttttt
tttt
QCCCKCKI
CzKxbel
xxxCzQxCzxbel
xNQxCzN
xbelxzpxbel
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
23
Kalman Filter Algorithm
1. Algorithm Kalman_filter( t-1, t-1, ut, zt):
2. Prediction:
3.
4.
5. Correction:
6.
7.
8.
9. Return t, t
ttttt uBA 1
t
T
tttt RAA 1
1)( t
T
ttt
T
ttt QCCCK
)( tttttt CzK
tttt CKI )(
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
Kalman Filter Algorithm
16.02.2015 Dr. Ahmad Kamal Nasir 24
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
Kalman Filter Algorithm
16.02.2015 Dr. Ahmad Kamal Nasir 25
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
26
The Prediction-Correction-Cycle
t
T
tttt
ttttt
tRAA
uBAxbel
1
1)(
2
,
222
1)(
tactttt
ttttt
ta
ubaxbel
Prediction
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
27
The Prediction-Correction-Cycle
1)(,)(
)()(
t
T
ttt
T
ttttttt
tttttt
t QCCCKCKI
CzKxbel
2
,
2
2
22 ,
)1(
)()(
tobst
tt
ttt
ttttt
t KK
zKxbel
Correction
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
28
The Prediction-Correction-Cycle
1)(,)(
)()(
t
T
ttt
T
ttttttt
tttttt
t QCCCKCKI
CzKxbel
2
,
2
2
22 ,
)1(
)()(
tobst
tt
ttt
ttttt
t KK
zKxbel
t
T
tttt
ttttt
tRAA
uBAxbel
1
1)(
2
,
222
1)(
tactttt
ttttt
ta
ubaxbel
Correction
Prediction
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
29
Kalman Filter Summary
• Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2)
• Optimal for linear Gaussian systems!
• Most robotics systems are nonlinear!
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
30
Nonlinear Dynamic Systems
• Most realistic robotic problems involve nonlinear functions
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
31
Linearity Assumption Revisited
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
32
Non-linear Function
16.02.2015 Dr. Ahmad Kamal Nasir
Non-Guassian
• The non-linear functions lead to non-Gaussian distributions
• Kalman filter is not applicable anymore!
What can be done to resolve this?
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
33
• Prediction:
• Correction:
EKF Linearization: First Order Taylor Series Expansion
)(),(),(
)(),(
),(),(
1111
11
1
111
ttttttt
tt
t
tttttt
xGugxug
xx
ugugxug
)()()(
)()(
)()(
ttttt
tt
t
ttt
xHhxh
xx
hhxh
16.02.2015 Dr. Ahmad Kamal Nasir
Jacobian matrices
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
Reminder: Jacobian Matrix
• It is a non-square matrix in 𝑛 × 𝑚 general
• Given a vector-valued function
• The Jacobian matrix is defined as
16.02.2015 Dr. Ahmad Kamal Nasir 34
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
Reminder: Jacobian Matrix
• It is the orientation of the tangent plane to the vector-valued function at a given point
• Generalizes the gradient of a scalar valued function
16.02.2015 Dr. Ahmad Kamal Nasir 35
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
36
• Prediction:
• Correction:
EKF Linearization: First Order Taylor Series Expansion
)(),(),(
)(),(
),(),(
1111
11
1
111
ttttttt
tt
t
tttttt
xGugxug
xx
ugugxug
)()()(
)()(
)()(
ttttt
tt
t
ttt
xHhxh
xx
hhxh
16.02.2015 Dr. Ahmad Kamal Nasir
Linear functions
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
37
EKF Linearization
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
38
EKF Linearization (Cont.)
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
39
EKF Algorithm
1. Extended_Kalman_filter ( t-1, t-1, ut, zt):
2. Prediction:
3.
4.
5. Correction:
6.
7.
8.
9. Return t, t
),( 1 ttt ug
t
T
tttt RGG 1
1)( t
T
ttt
T
ttt QHHHK
))(( ttttt hzK
tttt HKI )(
1
1),(
t
ttt
x
ugG
t
tt
x
hH
)(
ttttt uBA 1
t
T
tttt RAA 1
1)( t
T
ttt
T
ttt QCCCK
)( tttttt CzK
tttt CKI )(
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
Google Car – Tracking using KF
16.02.2015 Dr. Ahmad Kamal Nasir 40
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
41
Landmark-based Localization
16.02.2015 Dr. Ahmad Kamal Nasir
EKF localization with landmarks (point features)
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
42
1. EKF_localization ( t-1, t-1, ut, zt, m): Prediction:
3.
5.
6.
7.
8.
),( 1 ttt ug T
ttt
T
tttt 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
2
21
||||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
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
43
1. EKF_localization ( t-1, t-1, ut, zt, m): Correction:
3.
5.
6.
7.
8.
9.
10.
)ˆ( 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
t
T
tttt QHHS
1 t
T
ttt SHK
2
2
0
0
r
r
tQ
Predicted measurement mean
Pred. measurement covariance
Kalman gain
Updated mean
Updated covariance
Jacobian of h w.r.t location
16.02.2015 Dr. Ahmad Kamal Nasir
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
Summary
• Recursive State Estimation: Bayes Filter
• Linear Kalman Filter
• Extended Kalman Filter
– Works well in practice for moderate nonlinearities
16.02.2015 Dr. Ahmad Kamal Nasir 44
EE565: Mobile Robotics Module 2: Sensor Fusion and State Estimation
Questions
16.02.2015 Dr. Ahmad Kamal Nasir 45