markov localization & bayes filtering 1 with kalman filters discrete filters particle filters...

Markov Localization & Bayes Filtering

1

with

Kalman Filters

Discrete Filters

Particle Filters

Slides adapted from Thrun et al.,

Probabilistic Robotics

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

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

Markov Localization

4

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

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

Markov Localization

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

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

Markov Localization

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

9

Bayes Formula

evidence

prior likelihood

)(

)()|()(

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

yP

xPxyPyxP

xPxyPyPyxPyxP

10

Bayes Rule with Background Knowledge

)|(

)|(),|(),|(

zyP

zxPzxyPzyxP

11

Normalization

)()|(

1)(

)()|()(

)()|()(

1

xPxyPyP

xPxyPyP

xPxyPyxP

x

yx

xyx

yx

yxPx

xPxyPx

|

|

|

aux)|(:

aux

1

)()|(aux:

Algorithm:

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

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

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

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

•Prediction

•Correction

111 )(),|()( tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel

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

18

Bayes Filters are Familiar!

• Kalman filters

• Particle filters

• Hidden Markov models

• Dynamic Bayesian networks

• Partially Observable Markov Decision Processes (POMDPs)

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

19

SA-1

Probabilistic Robotics

Bayes Filter Implementations

Gaussian filters

),(~),(~ 22

2

abaNYbaXY

NX

Linear transform of Gaussians

2

2)(

2

1

2

2

1)(

:),(~)(

x

exp

Nxp

-

Univariate

Gaussians

• 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

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

24

0000 ,;)( xNxbel

Linear Gaussian Systems: Initialization

• Initial belief is normally distributed:

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

26

• Observations are linear function of state plus additive noise:

tttt xCz

Linear Gaussian Systems: Observations

tttttt QxCzNxzp ,;)|(

ttttttt

tttt

xNQxCzN

xbelxzpxbel

,;~,;~

)()|()(

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

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!

29

Nonlinear Dynamic Systems

•Most realistic robotic problems involve nonlinear functions

),( 1 ttt xugx

)( tt xhz

30

Linearity Assumption Revisited

31

Non-linear Function

32

EKF Linearization (1)

33

EKF Linearization (2)

34

EKF Linearization (3)

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

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

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]

38

Landmark-based Localization

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!

40

• [Arras et al. 98]:

• Laser range-finder and vision

• High precision (<1cm accuracy)

Kalman Filter-based System

[Courtesy of Kai Arras]

41

Multi-hypothesisTracking

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

43

MHT: Implemented System (2)

Courtesy of P. Jensfelt and S. Kristensen

SA-1

Probabilistic Robotics

Bayes Filter Implementations

Discrete filters

45

Piecewise Constant

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

47

Grid-based Localization

48

Sonars and Occupancy Grid Map

SA-1

Probabilistic Robotics

Bayes Filter Implementations

Particle filters

Sample-based Localization (sonar)

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

Weight samples: w = f / g

Importance Sampling

Importance Sampling with Resampling:Landmark Detection Example

Particle Filters

)|()(

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

xzpxBel

xBelxzpw

xBelxzpxBel

Sensor Information: Importance Sampling

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

Robot Motion

)|()(

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

xzpxBel

xBelxzpw

xBelxzpxBel

Sensor Information: Importance Sampling

Robot Motion

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

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

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

Start

Motion Model Reminder

Proximity Sensor Model Reminder

Laser sensor Sonar sensor

63

Initial Distribution

64

After Incorporating Ten Ultrasound Scans

65

After Incorporating 65 Ultrasound Scans

66

Estimated Path

Localization for AIBO robots

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

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

70

Global Localization

71

Kidnapping the Robot

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.