markov localization & bayes filtering

72
Markov Localization & Bayes Filtering 1 with Kalman Filters Discrete Filters Particle Filters Slides adapted from Thrun et al., Probabilistic Robotics

Upload: joben

Post on 20-Jan-2016

48 views

Category:

Documents


0 download

DESCRIPTION

Markov Localization & Bayes Filtering. with Kalman Filters Discrete Filters Particle Filters. Slides adapted from Thrun et al., Probabilistic Robotics. Control Scheme for Autonomous Mobile Robot. Control Scheme for Autonomous Mobile Robot – the plan. - PowerPoint PPT Presentation

TRANSCRIPT

Page 1: Markov Localization                      & Bayes Filtering

Markov Localization & Bayes Filtering

1

with

Kalman Filters

Discrete Filters

Particle Filters

Slides adapted from Thrun et al.,

Probabilistic Robotics

Page 2: Markov Localization                      & Bayes Filtering

Faculties of Engineering & Computer Science 2

Autonomous RoboticsCSCI 6905 / Mech 6905 – Section 3

Dalhousie Fall 2011 / 2012 Academic Term

• Introduction • Motion• Perception• Control• Concluding Remarks• LEGO Mindstorms

Control Scheme for AutonomousMobile Robot

Page 3: Markov Localization                      & Bayes Filtering

Faculties of Engineering & Computer Science 3

Autonomous RoboticsCSCI 6905 / Mech 6905 – Section 3

Dalhousie Fall 2011 / 2012 Academic Term

• Introduction • Motion• Perception• Control• Concluding Remarks• LEGO Mindstorms

Control Scheme for AutonomousMobile Robot – the plan

– Thomas will cover generalized Bayesian filters for localization next week

– Mae sets up the background for him, today, by discussing motion and sensor models as well as robot control

– Mae then follows on Bayesian filters to do a specific example, underwater SLAM

Page 4: Markov Localization                      & Bayes Filtering

Markov Localization

4

The robot doesn’t know where it is. Thus, a reasonable initial believe of it’s position is a uniform distribution.

Page 5: Markov Localization                      & Bayes Filtering

Markov Localization

5

A sensor reading is made (USE SENSOR MODEL) indicating a door at certain locations (USE MAP). This sensor reading should be integrated with prior believe to update our believe (USE BAYES).

Page 6: Markov Localization                      & Bayes Filtering

Markov Localization

6The robot is moving (USE MOTION MODEL) which adds noise.

Page 7: Markov Localization                      & Bayes Filtering

Markov Localization

7

A new sensor reading (USE SENSOR MODEL) indicates a door at certain locations (USE MAP). This sensor reading should be integrated with prior believe to update our believe (USE BAYES).

Page 8: Markov Localization                      & Bayes Filtering

Markov Localization

8The robot is moving (USE MOTION MODEL) which adds noise. …

Page 9: Markov Localization                      & Bayes Filtering

9

Bayes Formula

evidence

prior likelihood

)(

)()|()(

)()|()()|(),(

yP

xPxyPyxP

xPxyPyPyxPyxP

Page 10: Markov Localization                      & Bayes Filtering

10

Bayes Rule with Background Knowledge

)|(

)|(),|(),|(

zyP

zxPzxyPzyxP

Page 11: Markov Localization                      & Bayes Filtering

11

Normalization

)()|(

1)(

)()|()(

)()|()(

1

xPxyPyP

xPxyPyP

xPxyPyxP

x

yx

xyx

yx

yxPx

xPxyPx

|

|

|

aux)|(:

aux

1

)()|(aux:

Algorithm:

Page 12: Markov Localization                      & Bayes Filtering

12

Recursive Bayesian Updating

),,|(

),,|(),,,|(),,|(

11

11111

nn

nnnn

zzzP

zzxPzzxzPzzxP

Markov assumption: zn is independent of z1,...,zn-1 if we know x.

)()|(

),,|()|(

),,|(

),,|()|(),,|(

...1...1

11

11

111

xPxzP

zzxPxzP

zzzP

zzxPxzPzzxP

ni

in

nn

nn

nnn

Page 13: Markov Localization                      & Bayes Filtering

13

Putting oberservations and actions together: Bayes Filters• 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

Page 14: Markov Localization                      & Bayes Filtering

14

Graphical Representation and Markov Assumption

Underlying Assumptions• Static world• Independent noise• Perfect model, no approximation errors

),|(),,|( 1:1:11:1 ttttttt uxxpuzxxp )|(),,|( :1:1:0 tttttt xzpuzxzp

Page 15: Markov Localization                      & Bayes Filtering

15111 )(),|()|( ttttttt dxxBelxuxPxzP

Bayes 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

Page 16: Markov Localization                      & Bayes Filtering

•Prediction

•Correction

111 )(),|()( tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel

Page 17: Markov Localization                      & Bayes Filtering

17

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

Page 18: Markov Localization                      & Bayes Filtering

18

Bayes Filters are Familiar!

• Kalman filters

• Particle filters

• Hidden Markov models

• Dynamic Bayesian networks

• Partially Observable Markov Decision Processes (POMDPs)

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel

Page 19: Markov Localization                      & Bayes Filtering

19

Page 20: Markov Localization                      & Bayes Filtering

SA-1

Probabilistic Robotics

Bayes Filter Implementations

Gaussian filters

Page 21: Markov Localization                      & Bayes Filtering

),(~),(~ 22

2

abaNYbaXY

NX

Linear transform of Gaussians

2

2)(

2

1

2

2

1)(

:),(~)(

x

exp

Nxp

-

Univariate

Gaussians

Page 22: Markov Localization                      & Bayes Filtering

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

),(~),(~ TAABANY

BAXY

NX

Multivariate Gaussians

12

11

221

11

21

221

222

111 1,~)()(

),(~

),(~

NXpXpNX

NX

Page 23: Markov Localization                      & Bayes Filtering

23

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

Page 24: Markov Localization                      & Bayes Filtering

24

0000 ,;)( xNxbel

Linear Gaussian Systems: Initialization

• Initial belief is normally distributed:

Page 25: Markov Localization                      & Bayes Filtering

25

• 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

Page 26: Markov Localization                      & Bayes Filtering

26

• Observations are linear function of state plus additive noise:

tttt xCz

Linear Gaussian Systems: Observations

tttttt QxCzNxzp ,;)|(

ttttttt

tttt

xNQxCzN

xbelxzpxbel

,;~,;~

)()|()(

Page 27: Markov Localization                      & Bayes Filtering

27

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

tTtttt RAA 1

1)( tTttt

Tttt QCCCK

)( tttttt CzK

tttt CKI )(

Page 28: Markov Localization                      & Bayes Filtering

28

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!

Page 29: Markov Localization                      & Bayes Filtering

29

Nonlinear Dynamic Systems

•Most realistic robotic problems involve nonlinear functions

),( 1 ttt xugx

)( tt xhz

Page 30: Markov Localization                      & Bayes Filtering

30

Linearity Assumption Revisited

Page 31: Markov Localization                      & Bayes Filtering

31

Non-linear Function

Page 32: Markov Localization                      & Bayes Filtering

32

EKF Linearization (1)

Page 33: Markov Localization                      & Bayes Filtering

33

EKF Linearization (2)

Page 34: Markov Localization                      & Bayes Filtering

34

EKF Linearization (3)

Page 35: Markov Localization                      & Bayes Filtering

35

•Prediction:

•Correction:

EKF Linearization: First Order Taylor Series Expansion

)(),(),(

)(),(

),(),(

1111

111

111

ttttttt

ttt

tttttt

xGugxug

xx

ugugxug

)()()(

)()(

)()(

ttttt

ttt

ttt

xHhxh

xx

hhxh

Page 36: Markov Localization                      & Bayes Filtering

36

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

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 )(

Page 37: Markov Localization                      & Bayes Filtering

37

Localization

• 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]

Page 38: Markov Localization                      & Bayes Filtering

38

Landmark-based Localization

Page 39: Markov Localization                      & Bayes Filtering

39

EKF 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!

Page 40: Markov Localization                      & Bayes Filtering

40

• [Arras et al. 98]:

• Laser range-finder and vision

• High precision (<1cm accuracy)

Kalman Filter-based System

[Courtesy of Kai Arras]

Page 41: Markov Localization                      & Bayes Filtering

41

Multi-hypothesisTracking

Page 42: Markov Localization                      & Bayes Filtering

42

• 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 MHT

Page 43: Markov Localization                      & Bayes Filtering

43

MHT: Implemented System (2)

Courtesy of P. Jensfelt and S. Kristensen

Page 44: Markov Localization                      & Bayes Filtering

SA-1

Probabilistic Robotics

Bayes Filter Implementations

Discrete filters

Page 45: Markov Localization                      & Bayes Filtering

45

Piecewise Constant

Page 46: Markov Localization                      & Bayes Filtering

46

Discrete Bayes Filter Algorithm

1. Algorithm Discrete_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

'

)'()',|()('x

xBelxuxPxBel

Page 47: Markov Localization                      & Bayes Filtering

47

Grid-based Localization

Page 48: Markov Localization                      & Bayes Filtering

48

Sonars and Occupancy Grid Map

Page 49: Markov Localization                      & Bayes Filtering

SA-1

Probabilistic Robotics

Bayes Filter Implementations

Particle filters

Page 50: Markov Localization                      & Bayes Filtering

Sample-based Localization (sonar)

Page 51: Markov Localization                      & Bayes Filtering

Represent belief by random samples

Estimation of non-Gaussian, nonlinear processes

Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter, Particle filter

Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96]

Computer vision: [Isard and Blake 96, 98] Dynamic Bayesian Networks: [Kanazawa et al., 95]d

Particle Filters

Page 52: Markov Localization                      & Bayes Filtering

Weight samples: w = f / g

Importance Sampling

Page 53: Markov Localization                      & Bayes Filtering

Importance Sampling with Resampling:Landmark Detection Example

Page 54: Markov Localization                      & Bayes Filtering

Particle Filters

Page 55: Markov Localization                      & Bayes Filtering

)|()(

)()|()()|()(

xzpxBel

xBelxzpw

xBelxzpxBel

Sensor Information: Importance Sampling

Page 56: Markov Localization                      & Bayes Filtering

'd)'()'|()( , xxBelxuxpxBel

Robot Motion

Page 57: Markov Localization                      & Bayes Filtering

)|()(

)()|()()|()(

xzpxBel

xBelxzpw

xBelxzpxBel

Sensor Information: Importance Sampling

Page 58: Markov Localization                      & Bayes Filtering

Robot Motion

'd)'()'|()( , xxBelxuxpxBel

Page 59: Markov Localization                      & Bayes Filtering

1. Algorithm particle_filter( St-1, ut-1 zt):

2.

3. For Generate new samples

4. Sample index j(i) from the discrete distribution given by wt-

1

5. Sample from using and

6. Compute importance weight

7. Update normalization factor

8. Insert

9. For

10. Normalize weights

Particle Filter Algorithm

0, tS

ni 1

},{ it

ittt wxSS

itw

itx ),|( 11 ttt uxxp )(

1ij

tx 1tu

)|( itt

it xzpw

ni 1

/it

it ww

Page 60: Markov Localization                      & Bayes Filtering

draw xit1 from Bel(xt1)

draw xit from p(xt | xi

t1,ut1)

Importance factor for xit:

)|()(),|(

)(),|()|(ondistributi proposal

ondistributitarget

111

111

tt

tttt

tttttt

it

xzpxBeluxxp

xBeluxxpxzp

w

1111 )(),|()|()( tttttttt dxxBeluxxpxzpxBel

Particle Filter Algorithm

Page 61: Markov Localization                      & Bayes Filtering

Start

Motion Model Reminder

Page 62: Markov Localization                      & Bayes Filtering

Proximity Sensor Model Reminder

Laser sensor Sonar sensor

Page 63: Markov Localization                      & Bayes Filtering

63

Initial Distribution

Page 64: Markov Localization                      & Bayes Filtering

64

After Incorporating Ten Ultrasound Scans

Page 65: Markov Localization                      & Bayes Filtering

65

After Incorporating 65 Ultrasound Scans

Page 66: Markov Localization                      & Bayes Filtering

66

Estimated Path

Page 67: Markov Localization                      & Bayes Filtering

Localization for AIBO robots

Page 68: Markov Localization                      & Bayes Filtering

68

Limitations

•The approach described so far is able to • track the pose of a mobile robot and to• globally localize the robot.

•How can we deal with localization errors (i.e., the kidnapped robot problem)?

Page 69: Markov Localization                      & Bayes Filtering

69

Approaches

•Randomly insert samples (the robot can be teleported at any point in time).

• Insert random samples proportional to the average likelihood of the particles (the robot has been teleported with higher probability when the likelihood of its observations drops).

Page 70: Markov Localization                      & Bayes Filtering

70

Global Localization

Page 71: Markov Localization                      & Bayes Filtering

71

Kidnapping the Robot

Page 72: Markov Localization                      & Bayes Filtering

73

Summary

• Particle filters are an implementation of recursive Bayesian filtering

• They represent the posterior by a set of weighted samples.

• In the context of localization, the particles are propagated according to the motion model.

• They are then weighted according to the likelihood of the observations.

• In a re-sampling step, new particles are drawn with a probability proportional to the likelihood of the observation.