1

Robot Localization Using Bayesian Methods

Stochastic Processes Mini Conference Winter 2011EE 670 - Prof. Brian Mazzeo

Amin NazaranStephen Quebe

2

Presentation OutlineRobot LocalizationModeling Robot Localization as a Stochastic

Process.Bayesian Estimation and Filtering.The Extended Kalman Filter.Extended Kalman Filter Simulation Results.Conclusions.

3

Robot LocalizationIn order for a mobile robot to complete many

meaningful tasks, it must be able to identify and control its position in an environment.

“Using sensory information to locate the robot in its environment is the most fundamental problem in robotics [1].”

4

The Localization ProblemGiven a map of an environment and a

sequence of sensor measurements and control inputs, estimate the robot’s pose.

5

The Localization ProblemInputs OutputsRobot initial pose.

Control inputs.

Observations.

Map feature or landmarks.

Estimated robot pose.

X

Y

O

θ

( , )x y

robot's s ta te : xyθ

6

Robot Motion and Observation Models

X

Y

O

θ

( , )x y

robot's s ta te : xyθ

7

Modeling Robot Localization as a Stochastic ProcessOne approach to solving this problem is by

modeling the robot’s control inputs, observations using a Markov Chain.

8

The Markov AssumptionThe Markov assumption states that if we know

the current state of the robot, past and future states are conditionally independent of one another.

In other words. If we know where the robot is now, then knowing where the robot was 5 minutes ago doesn’t give us any more information than we already have, regarding it’s current state.

The arrows on Dynamic Bayes Network show this conditional independence.

9

Stochastic Motion ModelThe robot motion model describes the robot’s

pose as a function of it’s previous pose and control inputs.

The observation model describes the robot’s sensor measurements as a function of the robot’s position and the landmark position.

10

Stochastic Motion Model Bayes Network

11

Bayesian Estimation and FilteringIt is a recursive algorithm. At time t, given

the belief at time t-1 belt-1(xr), the last motion control ut-1 and the last measurement zt, determine the new belief belt(xr) as follows:Motion model

Measurement model

12

Bayesian Estimation: Prediction

Based on the total probability theorem:

where Bi, i=1,2,... is a partition of W. In the continuous case:

(discrete case)

Motion modelRobot pose space

13

The Extended Kalman Filter (EKF)The Extended Kalman Filter is one way to

apply Bayesian estimation techniques to robot localization and mapping.

The Kalman filter is the optimal Least Mean Squares estimator of a linear Gaussian system.

The Extended Kalman filter is a way of using the Kalman filter with non-linear models by approximating the model.

14

EKF Assumptions and ViolationsAssumptions:

Gaussian noise and uncertainty.Linear approximations are good.Markov assumption or complete state assumption holds.

Violations:Data association create Non-Gaussian uncertainties.With large time steps or angles the linear approximation

is poor.If the estimate becomes unstable or overconfident the

Markov assumption is violated by a poor estimate.If the robot is “bumped” or moved by something not in

the model, the Markov is also violated.

15

EKF Assumptions and Violations

EKF Algorithm1. EKF_localization ( mt-1, St-1, ut, zt, m):

Prediction:

,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

Jacobian of g w.r.t location

Jacobian of g w.r.t control

17

EKF Algorithm Continued17

17

),( 1 ttt ug Tttt

Ttttt VMVGG 1

2

43

221

||||0

0||||

tt

ttt

v

vM

Motion noise

Predicted mean

Predicted covariance

18

EKF Measurement Update

Based on the Bayes Rule:

Measurement modelNormalizing factor

Taking:

We have:

i.e. also:

19

1. EKF_localization ( mt-1, St-1, ut, zt, m):

Correction:

2.

3.

4.

5.

6.

)ˆ( it

it

it

it

it zzK

tit

itt HKI

,

,

,

,

,

,),(

t

it

t

it

yt

it

yt

it

xt

it

xt

it

t

tit

rrr

x

mhH

,,,,,

2,,

2,,

,2atanˆ

txtjxytjy

ytjyxtjxit

mm

mmz

t

Titt

it

it QHHS

1 i

t

Titt

it 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

20

EKF Simulation ResultsNormal operation.Overconfident prediction.Overconfident measurement.Large time steps where linearization fails.External bump where Markov assumption

fails.

21

Simulation ResultsShow simulation results in real time by

opening matlab.

22

ConclusionsThe critical assumption in the stochastic model

is the Markov assumption. This assumption is restrictive but probably cannot be avoided in any real world scenario.

The Extended Kalman Filter implementation is fast and remains consistent under normal conditions.

In the real world the model can be adjusted to reduce and recover from failure.

The robot must be able to recognize and recover from inevitable failure (the lost robot problem).

23

Thank You For Your AttentionQuestions?

24

References[1]: I.J. Cox. Blanche—an experiment in guidance and navigation of an autonomous robot vehicle. IEEE Transactions on Robotics and Automation, vol.7,NO.2 ,pp.193–204, 1991.[2] S. Thrun,W. Burgard, and D.Fox, “Probabilistic Robotics”, MIT press: Cambridge, 1967.