Stochastic Processes Mini Conference Winter 2011
EE 670 - Prof. Brian Mazzeo
Amin Nazaran
Stephen Quebe
1
Presentation Outline
 Robot Localization
 Modeling Robot Localization as a Stochastic Process.
 Bayesian Estimation and Filtering.
 The Extended Kalman Filter.
 Extended Kalman Filter Simulation Results.
 Conclusions.
2
Robot Localization
 In 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].”
3
The Localization Problem
 Given a map of an environment and a sequence of
sensor measurements and control inputs, estimate the
robot’s pose.
4
The Localization Problem
Inputs
Outputs
 Robot initial pose.
 Estimated robot pose.
θ
( ,x )y
 Control inputs.
Y
 Observations.
O
x
robot's state : y
θ
X
 Map feature or landmarks.
5
Robot Motion and Observation
Models
θ
( ,x )y
Y
O
x
robot's state : y
θ
X
6
Modeling Robot Localization as a
Stochastic Process
 One approach to solving this problem is by modeling
the robot’s control inputs, observations using a
Markov Chain.
7
The Markov Assumption
 The 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.
8
Stochastic Motion Model
 The 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.
9
Stochastic Motion Model Bayes
Network
10
Bayesian Estimation and Filtering
 It 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
11
Bayesian Estimation: Prediction
Robot pose space
Motion model
Based on the total probability theorem:
(discrete case)
where Bi, i=1,2,... is a partition of W. In the continuous case:
12
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.
13
EKF Assumptions and Violations
 Assumptions:
 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.
14
EKF Assumptions and Violations
15
EKF Algorithm
1.
EKF_localization ( mt-1, St-1, ut, zt, m):
Prediction:
3. Gt 
5.
Vt

g (ut , mt 1 )
xt 1
 x'

 mt 1, x
 y '
 
 mt 1, x
  '

 mt 1, x
g (ut , mt 1 )
ut
 x'

 vt
 y '
 
 vt
  '
 v
 t
x'
mt 1, y
y '
mt 1, y
 '
mt 1, y
x' 

t 
y ' 

t 
 ' 
t 
x' 
mt 1, 
y ' 
 Jacobian of g w.r.t location
mt 1, 
 ' 

mt 1, 
Jacobian of g w.r.t control
EKF Algorithm Continued
 1 | vt |  2 | t |2
M t  
0



2
 3 | vt |  4 | t | 
mt  g (ut , mt 1 )
St  Gt St 1GtT  Vt MtVtT
0
Motion noise
17
Predicted mean
Predicted covariance
17
17
EKF Measurement Update
Normalizing factor
Measurement model
Based on the Bayes Rule:
i.e. also:
Taking:
We have:
18
EKF_localization ( mt-1, St-1, ut, zt, m):
1.
Correction:
3.
zˆ
i
t
2
2


mx , j  mt , x   my , j  mt , y  

Predicted measurement mean

 atan2m  m , m  m   m 
y, j
t,y
x, j
t,x
t , 


h( mt , m)
xt
 rti

 m
  t ,ix
t

 m
 t,x
5.
H ti
6.
  r2 0 

Qt  
2
 0 r 
7.
S ti  H ti St H ti  Qt
T
8. K ti  St H ti Sti
9.
rti
mt , y
ti
mt , y
rti
mt ,
ti
mt ,







Jacobian of h w.r.t location
T
Pred. measurement covariance
1
Kalman gain
mti  mti  Kti ( zti  zˆti )
10. St  I  Kti Hti St
Updated mean
Updated covariance
19
EKF Simulation Results
 Normal operation.
 Overconfident prediction.
 Overconfident measurement.
 Large time steps where linearization fails.
 External bump where Markov assumption fails.
20
Simulation Results
 Show simulation results in real time by opening
matlab.
21
Conclusions
 The 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).
22
Thank You For Your Attention
Questions?
23
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.
24