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

Robotica Robotica

Anno accademico 2006/2007

Davide Miglioremigliore@elet.polimi.it

01/12/200601/12/2006 Corso di Robotica 06/07Corso di Robotica 06/07Davide Migliore - migliore@elet.polimi.itDavide Migliore - migliore@elet.polimi.it

22/69/69

TodayToday

● Bayes Filter Review● Gaussian Filters:

– Kalman Filter– Extended Kalman Filter– Unscented Kalman Filter– Localization with Multi Hypothesis Tracking

33/69/69

Last lessonLast lesson

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 =

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 =

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 η

),,,|(

),,,,|()|(

−−

−∫=

dxuzuxP

xuzuxPxzP

ηTotal prob.

Markov111111 ),,,|(),|()|( −−−−∫= tttttttt dxzzuxPxuxPxzP η

Measurement update

)|(),|(),|(

zxPzxyPzyxP =

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 η

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 η

99/69/69

Bayes Filter ImplementationsBayes Filter Implementations

Gaussian filtersGaussian filters

1010/69/69

● Prediction

● Correction

Bayes Filter ReminderBayes Filter Reminder

111 )(),|()( −−−∫= tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel η=

1111/69/69

GaussiansGaussians

:),(~)(

−−=

-σ σ

Univariate

:)(~)(

μxΣμx

−−− −

Multivariate

1212/69/69

),(~),(~ 22

σµσµabaNY

NX+⇒

Properties of GaussiansProperties of Gaussians

⋅⇒

−− 22

212222

2111 1

,~)()(),(~

σσµ

σσσµ

σσσ

σµσµ

NXpXpNX

1313/69/69

● We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations.

),(~),(~ TAABANY

NXΣ+⇒

Multivariate GaussiansMultivariate Gaussians

Σ+ΣΣ+Σ

Σ+Σ+Σ

Σ⋅⇒

−− 12

111 1,~)()(

),(~µµ

NXpXpNX

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

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.

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.

1616/69/69

Kalman Filter Updates in 1DKalman Filter Updates in 1DInitial Belief Measurement

Belief after measurement

1717/69/69

Kalman Filter Updates in 1DKalman Filter Updates in 1D

1)(with )(

)()( −+ΣΣ=

Σ−=Σ−+=

= tTttt

ttttttt QCCCK

CzKxbel

µµµ

22 with )1(

tttttt K

zKxbel

σσσ

σσµµµ

−=−+=

1818/69/69

Kalman Filter Updates in 1DKalman Filter Updates in 1D

+Σ=Σ+=

tTtttt

tttttt RAA

uBAxbel

1)(µµ

= −2

,2221)(

tactttt

tttttt a

ubaxbel

σσσµµ

1919/69/69

Kalman Filter UpdatesKalman Filter Updates

2020/69/69

( )0000 ,;)( Σ= µxNxbel

Linear Gaussian Systems: InitializationLinear Gaussian Systems: Initialization

● Initial belief is normally distributed:

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

,;~,;~

)(),|()(

−−−−

−−−

Σ+⇓⇓

ttttttttt

tttttt

xNRuBxAxN

dxxbelxuxpxbel

2222/69/69

Linear Gaussian Systems: DynamicsLinear Gaussian Systems: Dynamics

( ) ( )

+Σ=Σ+=

−Σ−−

−−−−−=

Σ+⇓⇓

−−−−−−−

−−

−−−−

−−−

tTtttt

tttttt

ttttttT

tttttt

ttttttttt

tttttt

uBAxbel

uBxAxRuBxAxxbel

xNRuBxAxN

dxxbelxuxpxbel

1111111

1exp)(

,;~,;~

)(),|()(

2323/69/69

● Observations are linear function of state plus additive noise:

tttt xCz δ+=

Linear Gaussian Systems: ObservationsLinear Gaussian Systems: Observations

( )tttttt QxCzNxzp ,;)|( =

( ) ( )ttttttt

xNQxCzN

xbelxzpxbel

⇓⇓

,;~,;~

)()|()(

2424/69/69

Linear Gaussian Systems: ObservationsLinear Gaussian Systems: Observations

( ) ( )

)(with )(

1exp)()(

1exp)(

,;~,;~

)()|()(

−−

+ΣΣ=

Σ−=Σ−+=

−Σ−−

−−−=

⇓⇓

ttttttt

ttttttT

ttttttt

QCCCKCKI

CzKxbel

xxxCzQxCzxbel

xNQxCzN

xbelxzpxbel

µµµ

µµη

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 Σ−=Σ )(

2626/69/69

The Prediction-Correction-CycleThe Prediction-Correction-Cycle

+Σ=Σ+=

tTtttt

tttttt RAA

uBAxbel

1)(µµ

= −2

,2221)(

tactttt

tttttt a

ubaxbel σσσ

Prediction

2727/69/69

1)(,)(

)()( −+ΣΣ=

Σ−=Σ−+=

= tTttt

ttttttt QCCCK

CzKxbel

µµµ

22 ,)1(

tttttt K

zKxbel

σσσ

σσµµµ

−=−+=

Correction

2828/69/69

1)(,)(

)()( −+ΣΣ=

Σ−=Σ−+=

= tTttt

ttttttt QCCCK

CzKxbel

µµµ

22 ,)1(

tttttt K

zKxbel

σσσ

σσµµµ

−=−+=

+Σ=Σ+=

tTtttt

tttttt RAA

uBAxbel

1)(µµ

= −2

,2221)(

tactttt

tttttt a

ubaxbel σσσ

Correction

Prediction

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!

3030/69/69

Nonlinear Dynamic SystemsNonlinear Dynamic Systems

● Most realistic robotic problems involve nonlinear functions

),( 1−= ttt xugx

)( tt xhz =

3131/69/69

Linearity Assumption RevisitedLinearity Assumption Revisited

3232/69/69

Non-linear FunctionNon-linear Function

3333/69/69

EKF Linearization (1)EKF Linearization (1)

3434/69/69

EKF Linearization (2) EKF Linearization (2)

3535/69/69

EKF Linearization (3)EKF Linearization (3)

3636/69/69

● Prediction:

● Correction:

EKF Linearization: First Order EKF Linearization: First Order Taylor Series ExpansionTaylor Series Expansion

)(),(),(

),(),(

−−−−

−−−

−+≈

−∂

∂+≈

ttttttt

tttttt

xGugxug

ugugxug

µµµ

)()()(

µµµ

−+≈

−∂

∂+≈

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 Σ−=Σ )(

∂∂=

∂∂= )(µ

ttttt uBA += −1µµ

tTtttt RAA +Σ=Σ −1

1)( −+ΣΣ= tTttt

Tttt QCCCK

)( tttttt CzK µµµ −+=tttt CKI Σ−=Σ )(

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]

3939/69/69

Landmark-based LocalizationLandmark-based Localization

4040/69/69

1. EKF_localization ( µt-1, Σt-1, ut, zt, m):

Prediction:

13. ),( 1−= ttt ug µµTttt

Ttttt VMVGG +Σ=Σ −1

∂∂

−−−

µµµ

µµµµ

,1,1,1

∂∂

∂= −

ωθθω

( )( )

ωααωαα Motion noise

Jacobian of g w.r.t location

Predicted mean

Predicted covariance

Jacobian of g w.r.t control

4141/69/69

Correction:

10. )ˆ( ttttt zzK −+= µµ

( ) tttt HKI Σ−=Σ

∂∂∂∂

µϕµ

µϕµµ

( ) ( )( )

−−−−+−=

θµµµµµ

,2atanˆ

txtxyty

ytyxtxt

tTtttt QHHS +Σ=

1−Σ= tTttt SHK

rtQ σ

Predicted measurement mean

Pred. measurement covariance

Kalman gain

Updated mean

Updated covariance

Jacobian of h w.r.t location

4242/69/69

EKF Prediction StepEKF Prediction Step

4343/69/69

EKF Observation Prediction StepEKF Observation Prediction Step

4444/69/69

EKF Correction StepEKF Correction Step

4545/69/69

Estimation Sequence (1)Estimation Sequence (1)

4646/69/69

Estimation Sequence (2)Estimation Sequence (2)

4747/69/69

Comparison to GroundTruthComparison to GroundTruth

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!

4949/69/69

Linearization via Unscented Linearization via Unscented TransformTransform

EKF UKF

5050/69/69

UKF Sigma-Point Estimate (2)UKF Sigma-Point Estimate (2)

EKF UKF

5151/69/69

UKF Sigma-Point Estimate (3)UKF Sigma-Point Estimate (3)

EKF UKF

5252/69/69

Unscented TransformUnscented Transform

( ) nin

2,...,1for )(2

)1( 2000

==Σ+±=

+−++

λλµχ

βαλ

λµχ

Sigma points Weights

)( ii g χψ =

−−=Σ

µψµψ

Pass sigma points through nonlinear function

Recover mean and covariance

5353/69/69

UKF_localization ( µt-1, Σt-1, ut, zt, m):

Prediction:

( )( )

ωααωαα

rtQ σ

( ) ( )( )TTTt

at 000011 −− = µµ

at 111111 −−−−−− Σ−Σ+= γµγµµχ

xt ug 1, −+= χχχ

( ) ( )∑=

−−=ΣL

0,, µχµχ

0,χµ

Motion noise

Measurement noise

Augmented state mean

Augmented covariance

Sigma points

Prediction of sigma points

Predicted mean

Predicted covariance

5454/69/69

UKF_localization ( µt-1, Σt-1, ut, zt, m):

Correction:

( ) zt

xtt h χχ +=Ζ

imt wz

Measurement sigma points

Cross-covariance

Kalman gain

Updated mean

Updated covariance

( ) ( )Ttti

ict zzwS ˆˆ ,

0, −Ζ−Ζ= ∑

( ) ( )Ttti

zxt zw ˆ,

, −Ζ−=Σ ∑=

1, −Σ= tzx

)ˆ( ttttt zzK −+= µµ

Tttttt KSK−Σ=Σ

5555/69/69

Correction:

10. )ˆ( ttttt zzK −+= µµ

( ) tttt HKI Σ−=Σ

∂∂∂∂

µϕµ

µϕµµ

( ) ( )( )

−−−−+−=

θµµµµµ

,2atanˆ

txtxyty

ytyxtxt

tTtttt QHHS +Σ=

1−Σ= tTttt SHK

rtQ σ

Kalman gain

Updated mean

Updated covariance

Jacobian of h w.r.t location

5656/69/69

UKF Prediction StepUKF Prediction Step

5757/69/69

UKF Observation Prediction StepUKF Observation Prediction Step

5858/69/69

UKF Correction StepUKF Correction Step

5959/69/69

EKF Correction StepEKF Correction Step

6060/69/69

Estimation SequenceEstimation Sequence

EKF PF UKF

6161/69/69

UKF PF EKF PF

6262/69/69

UKF PF UKF PF

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!

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]

6565/69/69

Multi-hypothesisTracking

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

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 =

)()|()|(

HPHsPsHP ii

[Jensfelt et al. ’00]

6868/69/69

MHT: Implemented System (2)MHT: Implemented System (2)

Courtesy of P. Jensfelt and S. Kristensen

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

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

Documents

actuadores (robotica)

robotica industrial

robotica y leyes de la robotica

robotica -...

robotica educativa

robotica - intranet...

mywebsite design service...search engine optimization (seo)...

historia robotica

il migliore

la migliore

robotica avanzada

mywebsite design service...mywebsite design service ihre...

la donna nella robotica - unibo.it · la donna nella...

tutorial robotica

exceptional cleaning - 1and1 mywebsite case study

robotica -...

teoria robotica

robotica · robotica, systems control,”

robotica robotica robotica robotica

cartilla robotica