robotica - intranet deibhome.deib.polimi.it/restelli/mywebsite/pdf/lezione6.pdf · 01/12/2006 corso...

69
Robotica Robotica Anno accademico 2006/2007 Davide Migliore [email protected]

Upload: dinhkhue

Post on 18-Oct-2018

213 views

Category:

Documents


0 download

TRANSCRIPT

Robotica Robotica

Anno accademico 2006/2007

Davide [email protected]

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

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

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