robotica - intranet deibhome.deib.polimi.it/restelli/mywebsite/pdf/lezione6.pdf · 01/12/2006 corso...
TRANSCRIPT
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
22/69/69
TodayToday
● Bayes Filter Review● Gaussian Filters:
– Kalman Filter– Extended Kalman Filter– Unscented Kalman Filter– Localization with Multi Hypothesis Tracking
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
33/69/69
Last lessonLast lesson
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
44/69/69
Bayes Filters: FrameworkBayes Filters: Framework● Given:
– Stream of observations z and action data u:
– Sensor model P(z|x).– Action model P(x|u,x’).– Prior probability of the system state P(x).
● Wanted: – Estimate of the state X of a dynamical system.– The posterior of the state is also called Belief:
),,,|()( 11 tttt zuzuxPxBel =
},,,{ 11 ttt zuzud =
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
55/69/69
Markov AssumptionMarkov Assumption
Underlying Assumptions● Static world● Independent noise● Perfect model, no approximation errors
),|(),,|( 1:1:11:1 ttttttt uxxpuzxxp −− =)|(),,|( :1:1:0 tttttt xzpuzxzp =
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
66/69/69111 )(),|()|( −−−∫= ttttttt dxxBelxuxPxzPη
Bayes FiltersBayes Filters
),,,|(),,,,|( 1111 ttttt uzuxPuzuxzP η=Bayes
z = observationu = actionx = state
),,,|()( 11 tttt zuzuxPxBel =
Markov ),,,|()|( 11 tttt uzuxPxzP η=
Markov11111 ),,,|(),|()|( −−−∫= tttttttt dxuzuxPxuxPxzP η
1111
111
),,,|(
),,,,|()|(
−−
−∫=
ttt
ttttt
dxuzuxP
xuzuxPxzP
ηTotal prob.
Markov111111 ),,,|(),|()|( −−−−∫= tttttttt dxzzuxPxuxPxzP η
Measurement update
)|(
)|(),|(),|(
zyP
zxPzxyPzyxP =
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
77/69/69
Bayes Filter Algorithm
1. Algorithm Bayes_filter( Bel(x),d ):2. η=0
3. If d is a perceptual data item z then4. For all x do5. 6. 7. For all x do8.
9. Else if d is an action data item u then10. For all x do11.
12. Return Bel’(x)
)()|()(' xBelxzPxBel =)(' xBel+=ηη
)(')(' 1 xBelxBel −=η
')'()',|()(' dxxBelxuxPxBel ∫=
111 )(),|()|()( −−−∫= tttttttt dxxBelxuxPxzPxBel η
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
88/69/69
Bayes Filters are Familiar!Bayes Filters are Familiar!
● Kalman filters● Particle filters● Hidden Markov models● Dynamic Bayesian networks● Partially Observable Markov Decision Processes
(POMDPs)
111 )(),|()|()( −−−∫= tttttttt dxxBelxuxPxzPxBel η
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
99/69/69
Bayes Filter ImplementationsBayes Filter Implementations
Gaussian filtersGaussian filters
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
1010/69/69
● Prediction
● Correction
Bayes Filter ReminderBayes Filter Reminder
111 )(),|()( −−−∫= tttttt dxxbelxuxpxbel
)()|()( tttt xbelxzpxbel η=
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
1111/69/69
GaussiansGaussians
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
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
1212/69/69
),(~),(~ 22
2
σµσµabaNY
baXY
NX+⇒
+=
Properties of GaussiansProperties of Gaussians
++
++
⋅⇒
−− 22
21
222
21
21
122
21
22
212222
2111 1
,~)()(),(~
),(~
σσµ
σσσµ
σσσ
σµσµ
NXpXpNX
NX
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
1313/69/69
● We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations.
),(~),(~ TAABANY
BAXY
NXΣ+⇒
+=Σ
µµ
Multivariate GaussiansMultivariate Gaussians
Σ+ΣΣ+Σ
Σ+Σ+Σ
Σ⋅⇒
ΣΣ
−− 12
11
221
11
21
221
222
111 1,~)()(
),(~
),(~µµ
µµ
NXpXpNX
NX
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
1414/69/69
Discrete Kalman FilterDiscrete 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
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
1515/69/69
Components of a Kalman FilterComponents 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.
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
1616/69/69
Kalman Filter Updates in 1DKalman Filter Updates in 1DInitial Belief Measurement
Belief after measurement
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
1717/69/69
Kalman Filter Updates in 1DKalman Filter Updates in 1D
1)(with )(
)()( −+ΣΣ=
Σ−=Σ−+=
= tTttt
Tttt
tttt
ttttttt QCCCK
CKI
CzKxbel
µµµ
2,
2
2
22 with )1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
σσσ
σσµµµ
+=
−=−+=
=
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
1818/69/69
Kalman Filter Updates in 1DKalman Filter Updates in 1D
+Σ=Σ+=
=−
−
tTtttt
tttttt RAA
uBAxbel
1
1)(µµ
+=+=
= −2
,2221)(
tactttt
tttttt a
ubaxbel
σσσµµ
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
1919/69/69
Kalman Filter UpdatesKalman Filter Updates
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
2020/69/69
( )0000 ,;)( Σ= µxNxbel
Linear Gaussian Systems: InitializationLinear Gaussian Systems: Initialization
● Initial belief is normally distributed:
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
2121/69/69
● Dynamics are linear function of state and control plus additive noise:
tttttt uBxAx ε++= −1
Linear Gaussian Systems: DynamicsLinear Gaussian Systems: Dynamics
( )ttttttttt RuBxAxNxuxp ,;),|( 11 += −−
( ) ( )1111
111
,;~,;~
)(),|()(
−−−−
−−−
Σ+⇓⇓
= ∫
ttttttttt
tttttt
xNRuBxAxN
dxxbelxuxpxbel
µ
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
2222/69/69
Linear Gaussian Systems: DynamicsLinear Gaussian Systems: Dynamics
( ) ( )
+Σ=Σ+=
=
−Σ−−
−−−−−=
⇓
Σ+⇓⇓
=
−
−
−−−−−−−
−−
−
−−−−
−−−
∫
∫
tTtttt
tttttt
ttttT
tt
ttttttT
tttttt
ttttttttt
tttttt
RAA
uBAxbel
dxxx
uBxAxRuBxAxxbel
xNRuBxAxN
dxxbelxuxpxbel
1
1
1111111
11
1
1111
111
)(
)()(2
1exp
)()(2
1exp)(
,;~,;~
)(),|()(
µµ
µµ
η
µ
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
2323/69/69
● Observations are linear function of state plus additive noise:
tttt xCz δ+=
Linear Gaussian Systems: ObservationsLinear Gaussian Systems: Observations
( )tttttt QxCzNxzp ,;)|( =
( ) ( )ttttttt
tttt
xNQxCzN
xbelxzpxbel
Σ
⇓⇓
=
,;~,;~
)()|()(
µ
η
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
2424/69/69
Linear Gaussian Systems: ObservationsLinear Gaussian Systems: Observations
( ) ( )
1
11
)(with )(
)()(
)()(2
1exp)()(
2
1exp)(
,;~,;~
)()|()(
−
−−
+ΣΣ=
Σ−=Σ−+=
=
−Σ−−
−−−=
⇓
Σ
⇓⇓
=
tTttt
Tttt
tttt
ttttttt
tttT
ttttttT
tttt
ttttttt
tttt
QCCCKCKI
CzKxbel
xxxCzQxCzxbel
xNQxCzN
xbelxzpxbel
µµµ
µµη
µ
η
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
2525/69/69
Kalman Filter Algorithm Kalman Filter Algorithm
1. Algorithm Kalman_filter( µt-1, Σt-1, ut, zt):
3. Prediction:4. 5.
6. Correction:7. 8. 9.
10. Return µt, Σt
ttttt uBA += −1µµ
tTtttt RAA +Σ=Σ −1
1)( −+ΣΣ= tTttt
Tttt QCCCK
)( tttttt CzK µµµ −+=tttt CKI Σ−=Σ )(
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
2626/69/69
The Prediction-Correction-CycleThe Prediction-Correction-Cycle
+Σ=Σ+=
=−
−
tTtttt
tttttt RAA
uBAxbel
1
1)(µµ
+=+=
= −2
,2221)(
tactttt
tttttt a
ubaxbel σσσ
µµ
Prediction
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
2727/69/69
The Prediction-Correction-CycleThe Prediction-Correction-Cycle
1)(,)(
)()( −+ΣΣ=
Σ−=Σ−+=
= tTttt
Tttt
tttt
ttttttt QCCCK
CKI
CzKxbel
µµµ
2,
2
2
22 ,)1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
σσσ
σσµµµ
+=
−=−+=
=
Correction
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
2828/69/69
The Prediction-Correction-CycleThe Prediction-Correction-Cycle
1)(,)(
)()( −+ΣΣ=
Σ−=Σ−+=
= tTttt
Tttt
tttt
ttttttt QCCCK
CKI
CzKxbel
µµµ
2,
2
2
22 ,)1(
)()(
tobst
tt
ttt
tttttt K
K
zKxbel
σσσ
σσµµµ
+=
−=−+=
=
+Σ=Σ+=
=−
−
tTtttt
tttttt RAA
uBAxbel
1
1)(µµ
+=+=
= −2
,2221)(
tactttt
tttttt a
ubaxbel σσσ
µµ
Correction
Prediction
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
2929/69/69
Kalman Filter SummaryKalman 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!
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
3030/69/69
Nonlinear Dynamic SystemsNonlinear Dynamic Systems
● Most realistic robotic problems involve nonlinear functions
),( 1−= ttt xugx
)( tt xhz =
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
3131/69/69
Linearity Assumption RevisitedLinearity Assumption Revisited
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
3232/69/69
Non-linear FunctionNon-linear Function
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
3333/69/69
EKF Linearization (1)EKF Linearization (1)
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
3434/69/69
EKF Linearization (2) EKF Linearization (2)
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
3535/69/69
EKF Linearization (3)EKF Linearization (3)
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
3636/69/69
● Prediction:
● Correction:
EKF Linearization: First Order EKF Linearization: First Order Taylor Series ExpansionTaylor Series Expansion
)(),(),(
)(),(
),(),(
1111
111
111
−−−−
−−−
−−−
−+≈
−∂
∂+≈
ttttttt
ttt
tttttt
xGugxug
xx
ugugxug
µµ
µµµ
)()()(
)()(
)()(
ttttt
ttt
ttt
xHhxh
xx
hhxh
µµ
µµµ
−+≈
−∂
∂+≈
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
3737/69/69
EKF Algorithm EKF Algorithm
1. Extended_Kalman_filter( µt-1, Σt-1, ut, zt):
3. Prediction:4. 5.
6. Correction:7. 8. 9.
10. Return µt, Σt
),( 1−= ttt ug µµ
tTtttt RGG +Σ=Σ −1
1)( −+ΣΣ= tTttt
Tttt QHHHK
))(( ttttt hzK µµµ −+=
tttt HKI Σ−=Σ )(
1
1),(
−
−
∂∂=
t
ttt x
ugG
µ
t
tt x
hH
∂∂= )(µ
ttttt uBA += −1µµ
tTtttt RAA +Σ=Σ −1
1)( −+ΣΣ= tTttt
Tttt QCCCK
)( tttttt CzK µµµ −+=tttt CKI Σ−=Σ )(
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
3838/69/69
LocalizationLocalization
● 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]
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
3939/69/69
Landmark-based LocalizationLandmark-based Localization
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
4040/69/69
1. EKF_localization ( µt-1, Σt-1, ut, zt, m):
Prediction:
4.
8.
10.
12.
13. ),( 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
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
4141/69/69
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
tTtttt QHHS +Σ=
1−Σ= tTttt 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
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
4242/69/69
EKF Prediction StepEKF Prediction Step
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
4343/69/69
EKF Observation Prediction StepEKF Observation Prediction Step
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
4444/69/69
EKF Correction StepEKF Correction Step
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
4545/69/69
Estimation Sequence (1)Estimation Sequence (1)
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
4646/69/69
Estimation Sequence (2)Estimation Sequence (2)
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
4747/69/69
Comparison to GroundTruthComparison to GroundTruth
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
4848/69/69
EKF SummaryEKF 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!
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
4949/69/69
Linearization via Unscented Linearization via Unscented TransformTransform
EKF UKF
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
5050/69/69
UKF Sigma-Point Estimate (2)UKF Sigma-Point Estimate (2)
EKF UKF
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
5151/69/69
UKF Sigma-Point Estimate (3)UKF Sigma-Point Estimate (3)
EKF UKF
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
5252/69/69
Unscented TransformUnscented Transform
( ) nin
wwn
nw
nw
ic
imi
i
cm
2,...,1for )(2
1 )(
)1( 2000
=+
==Σ+±=
+−++
=+
==
λλµχ
βαλ
λλ
λµχ
Sigma points Weights
)( ii g χψ =
∑
∑
=
=
−−=Σ
=
n
i
Tiiic
n
i
iim
w
w
2
0
2
0
))(('
'
µψµψ
ψµ
Pass sigma points through nonlinear function
Recover mean and covariance
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
5353/69/69
UKF_localization ( µt-1, Σt-1, ut, zt, m):
Prediction:
( )( )
+
+=2
43
221
||||0
0||||
tt
ttt
v
vM
ωααωαα
=
2
2
0
0
r
rtQ σ
σ
( ) ( )( )TTTt
at 000011 −− = µµ
Σ=Σ
−
−
t
t
tat
Q
M
00
00
001
1
( )at
at
at
at
at
at 111111 −−−−−− Σ−Σ+= γµγµµχ
( )xt
utt
xt ug 1, −+= χχχ
( ) ( )∑=
−−=ΣL
i
T
txtit
xti
ict w
2
0,, µχµχ
∑=
=L
i
xti
imt w
2
0,χµ
Motion noise
Measurement noise
Augmented state mean
Augmented covariance
Sigma points
Prediction of sigma points
Predicted mean
Predicted covariance
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
5454/69/69
UKF_localization ( µt-1, Σt-1, ut, zt, m):
Correction:
( ) zt
xtt h χχ +=Ζ
∑=
Ζ=L
iti
imt wz
2
0,ˆ
Measurement sigma points
Predicted measurement mean
Pred. measurement covariance
Cross-covariance
Kalman gain
Updated mean
Updated covariance
( ) ( )Ttti
L
itti
ict zzwS ˆˆ ,
2
0, −Ζ−Ζ= ∑
=
( ) ( )Ttti
L
it
xti
ic
zxt zw ˆ,
2
0,
, −Ζ−=Σ ∑=
µχ
1, −Σ= tzx
tt SK
)ˆ( ttttt zzK −+= µµ
Tttttt KSK−Σ=Σ
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
5555/69/69
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
tTtttt QHHS +Σ=
1−Σ= tTttt 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
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
5656/69/69
UKF Prediction StepUKF Prediction Step
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
5757/69/69
UKF Observation Prediction StepUKF Observation Prediction Step
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
5858/69/69
UKF Correction StepUKF Correction Step
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
5959/69/69
EKF Correction StepEKF Correction Step
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
6060/69/69
Estimation SequenceEstimation Sequence
EKF PF UKF
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
6161/69/69
Estimation SequenceEstimation Sequence
UKF PF EKF PF
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
6262/69/69
Estimation SequenceEstimation Sequence
UKF PF UKF PF
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
6363/69/69
UKF SummaryUKF Summary
● Highly efficient: Same complexity as EKF, with a constant factor slower in typical practical applications
● Better linearization than EKF: Accurate in first two terms of Taylor expansion (EKF only first term)
● Derivative-free: No Jacobians needed
● Still not optimal!
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
6464/69/69
• [Arras et al. 98]:
• Laser range-finder and vision
• High precision (<1cm accuracy)
Kalman Filter-based SystemKalman Filter-based System
[Courtesy of Kai Arras]
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
6565/69/69
Multi-hypothesisTracking
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
6666/69/69
• Belief is represented by multiple hypotheses
• Each hypothesis is tracked by a Kalman filter
• Additional problems:
• Data association: Which observation corresponds to
which hypothesis?
• Hypothesis management: When to add / delete
hypotheses?
• Huge body of literature on target tracking, motion
correspondence etc.
Localization With MHTLocalization With MHT
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
6767/69/69
• Hypotheses are extracted from LRF scans
• Each hypothesis has probability of being the correct one:
• Hypothesis probability is computed using Bayes’ rule
• Hypotheses with low probability are deleted.
• New candidates are extracted from LRF scans.
MHT: Implemented System (1)MHT: Implemented System (1)
)}(,,ˆ{ iiii HPxH Σ=
},{ jjj RzC =
)(
)()|()|(
sP
HPHsPsHP ii
i =
[Jensfelt et al. ’00]
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
6868/69/69
MHT: Implemented System (2)MHT: Implemented System (2)
Courtesy of P. Jensfelt and S. Kristensen
01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - [email protected] Migliore - [email protected]
6969/69/69
MHT: Implemented System (3)MHT: Implemented System (3)Example runExample run
Map and trajectory
# hypotheses
#hypotheses vs. time
P(Hbest)
Courtesy of P. Jensfelt and S. Kristensen