1. Probability in Robotics

2. Trends in Robotics Research
Classical Robotics (mid-70’s)
exact models
no sensing necessary
Reactive Paradigm (mid-80’s)
no models
relies heavily on good sensing
Hybrids (since 90’s)
model-based at higher levels
reactive at lower levels
Probabilistic Robotics (since mid-90’s)
seamless integration of models and sensing
inaccurate models, inaccurate sensors

3. Advantages of Probabilistic Paradigm
Can accommodate inaccurate models
Can accommodate imperfect sensors
Robust in real-world applications
Best known approach to many hard robotics problems
Pays Tribute to Inherent Uncertainty
Know your own ignorance
Scalability
No need for “perfect” world model
Relieves programmers

4. Limitations of Probability
Computationally inefficient
Consider entire probability densities
Approximation
Representing continuous probability distributions.

5. Uncertainty Representation

6. Five Sources of Uncertainty
Environment
Dynamics
Approximate
Computation
Random
Action Effects
Inaccurate
Models
Sensor
Limitations

7. Real environments imply uncertainty in accuracy of
Why Probabilities
Real environments imply uncertainty in accuracy of
robot actions
sensor measurements
Robot accuracy and correct models are vital for successful operations
All available data must be used
A lot of data is available in the form of probabilities

8. What Probabilities Sensor parameters Sensor accuracy
Robot wheels slipping
Motor resolution limited
Wheel precision limited
Performance alternates based on temperature, etc.

9. Reasons for Motion Errors
different wheel diameters
ideal case
bump
carpet
and many more …

10. What Probabilities
These inaccuracies can be measured and modelled with random distributions
Single reading of a sensor contains more information given the prior probability distribution of sensor behavior than its actual value
Robot cannot afford throwing away this additional information!

11. More advanced concepts: Robot position and orientation (robot pose)‏
What Probabilities
More advanced concepts:
Robot position and orientation (robot pose)‏
Map of the environment
Planning and control
Action selection
Reasoning...

12. Localization, Where am I?
Perception
Odometry, Dead Reckoning
Localization base on external sensors, beacons or landmarks
Probabilistic Map Based Localization

13. Localization Methods Mathematic Background, Bayes Filter
Markov Localization:
Central idea: represent the robot’s belief by a probability distribution over possible positions, and uses Bayes’ rule and convolution to update the belief whenever the robot senses or moves
Markov Assumption: past and future data are independent if one knows the current state
Kalman Filtering
Central idea: posing localization problem as a sensor fusion problem
Assumption: gaussian distribution function
Particle Filtering
Central idea: Sample-based, nonparametric Filter
Monte-Carlo method
SLAM (simultaneous localization and mapping)
Multi-robot localization

14. Markov Localization Applying probability theory to robot localization
Markov localization uses an explicit, discrete representation for the probability of all position in the state space.
This is usually done by representing the environment by a grid or a topological graph with a finite number of possible states (positions).
During each update, the probability for each state (element) of the entire space is updated.

15. Markov Localization Example
Assume the robot position is one- dimensional
The robot is placed
somewhere in the
environment but it is not told its location
The robot queries its
sensors and finds out it is next to a door

16. Markov Localization Example
The robot moves one meter forward. To account for inherent noise in robot motion the new belief is smoother
The robot queries its
sensors and again it finds itself next to a door

17. Probabilistic Robotics
Falls in between model-based and behavior-based techniques
There are models, and sensor measurements, but they are assumed to be incomplete and insufficient for control
Statistics provides the mathematical glue to integrate models and sensor measurements
Basic Mathematics
Probabilities
Bayes rule
Bayes filters

18. Nature of Sensor Data
Range Data
Odometry Data

19. Environmental Uncertainty
Sensor inaccuracy
Environmental Uncertainty

20. How do we Solve Localization Uncertainty?
Represent beliefs as a probability density
Markov assumption
Pose distribution at time t conditioned on:
pose dist. at time t-1
movement at time t-1
sensor readings at time t
Discretize the density by
sampling

21. Probabilistic Action model
At every time step t:
UPDATE each sample’s new location based on movement
RESAMPLE the pose distribution based on sensor readings
p(st|at-1,st-1)
at-1
st-1
st-1
at-1
Continuous probability density Bel(st) after moving 40m (left figure) and 80m (right figure). Darker area has higher probablity.

22. Globalization
Localization without knowledge of start location

23. Probabilistic Robotics: Basic Idea
Key idea: Explicit representation of uncertainty using probability theory
Perception = state estimation
Action = utility optimization

24. Advantages and Pitfalls
Can accommodate inaccurate models
Can accommodate imperfect sensors
Robust in real-world applications
Best known approach to many hard robotics problems
Computationally demanding
False assumptions
Approximate

25. Axioms of Probability Theory
Pr(A) denotes probability that proposition A is true.

26. A Closer Look at Axiom 3 B

27. Using the Axioms

28. Discrete Random Variables
X denotes a random variable.
X can take on a finite number of values in {x1, x2, …, xn}.
P(X=xi), or P(xi), is the probability that the random variable X takes on value xi.
P( ) is called probability mass function.
E.g.

29. Continuous Random Variables
X takes on values in the continuum.
p(X=x), or p(x), is a probability density function.
E.g.
p(x) x

30. Joint and Conditional Probability
P(X=x and Y=y) = P(x,y)
If X and Y are independent then P(x,y) = P(x) P(y)
P(x | y) is the probability of x given y P(x | y) = P(x,y) / P(y) P(x,y) = P(x | y) P(y)
If X and Y are independent then P(x | y) = P(x)

31. Law of Total Probability
Discrete case
Continuous case

32. Thomas Bayes ( )
Clergyman and mathematician who first used probability inductively and established a mathematical basis for probability inference

33. Bayes Formula

34. Normalization

35. Conditioning
Total probability:
Bayes rule and background knowledge:

36. Simple Example of State Estimation
Suppose a robot obtains measurement z
What is P(open|z)?

37. Causal vs. Diagnostic Reasoning
P(open|z) is diagnostic.
P(z|open) is causal.
Often causal knowledge is easier to obtain.
Bayes rule allows us to use causal knowledge:
count frequencies!

38. Example z raises the probability that the door is open.
P(z|open) = 0.6 P(z|open) = 0.3
P(open) = P(open) = 0.5
z raises the probability that the door is open.

39. A Typical Pitfall Two possible locations x1 and x2 P(x1)=0.99
P(z|x2)=0.09 P(z|x1)=0.07

40. Combining Evidence Suppose our robot obtains another observation z2.
How can we integrate this new information?
More generally, how can we estimate P(x| z1...zn )?

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

42. Example: Second Measurement
P(z2|open) = 0.5 P(z2|open) = 0.6
P(open|z1)=2/3
z2 lowers the probability that the door is open.

43. actions carried out by the robot, actions carried out by other agents,
Often the world is dynamic since
actions carried out by the robot,
actions carried out by other agents,
or just the time passing by
change the world.
How can we incorporate such actions?

44. Typical Actions The robot turns its wheels to move
The robot uses its manipulator to grasp an object
Actions are never carried out with absolute certainty.
In contrast to measurements, actions generally increase the uncertainty.

45. Modeling Actions
To incorporate the outcome of an action u into the current “belief”, we use the conditional pdf
P(x|u,x’)
This term specifies the pdf that executing u changes the state from x’ to x.

46. Example: Closing the door

47. State Transitions P(x|u,x’) for u = “close door”:
If the door is open, the action “close door” succeeds in 90% of all cases.

48. Integrating the Outcome of Actions
Continuous case:
Discrete case:

49. Example: The Resulting Belief

50. Robot Environment Interaction
State transition probability
measurement probability

51. Bayes 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:

52. Markov Assumption Underlying Assumptions Static world
Independent noise
Perfect model, no approximation errors

53. Bayes Filters z = observation u = action x = state Bayes Markov
Total prob.
Markov
Markov

54. Bayes Filter Algorithm
Algorithm Bayes_filter( Bel(x),d ):
0
If d is a perceptual data item z then
For all x do
Else if d is an action data item u then
Return Bel’(x)

55. Bayes Filters are Familiar!
Kalman filters (a recursive Bayesian filter for multivariate normal distributions)
Particle filters (a sequential Monte Carlo (SMC) based technique, which models the PDF using a set of discrete points)
Hidden Markov models (Markov process with unknown parameters)
Dynamic Bayesian networks
Partially Observable Markov Decision Processes (POMDPs)‏

56. Summary
Bayes rule allows us to compute probabilities that are hard to assess otherwise
Under the Markov assumption, recursive Bayesian updating can be used to efficiently combine evidence
Bayes filters are a probabilistic tool for estimating the state of dynamic systems.

57. How all of this relates to Sensors and navigation?
Sensor fusion

58. Basic statistics – Statistical representation – Stochastic variable
Travel time, X = 5hours ±1hour
X can have many different values
Continous – The variable can have any value within the bounds
Discrete – The variable can have specific (discrete) values

59. Basic statistics – Statistical representation – Stochastic variable
Another way of describing the stochastic variable, i.e. by another form of bounds
Probability distribution
In 68%: x11 < X < x12
In 95%: x21 < X < x22
In 99%: x31 < X < x32
In 100%: - < X < 
The value to expect is the mean value => Expected value
How much X varies from its expected value => Variance

60. Expected value and Variance
The standard deviation X is the square root of the variance

61. Gaussian (Normal) distribution
By far the mostly used probability distribution because of its nice statistical and mathematical properties
What does it means if a specification tells that a sensor measures a distance [mm] and has an error that is normally distributed with zero mean and  = 100mm?
Normal distribution:
~68.3%
~95%
~99%
etc.

62. Estimate of the expected value and the variance from observations

63. Linear combinations (1)
X2 ~ N(m2, σ2)
X1 ~ N(m1, σ1)
Y ~ N(m1 + m2, sqrt(σ1 +σ2))
Since linear combination of Gaussian variables is another Gaussian variable, Y remains Gaussian if the s.v. are combined linearly!

64. Linear combinations (2)
We measure a distance by a device that have normally distributed errors,
Do we win something of making a lot of measurements and use the average value instead?
What will the expected value of Y be?
What will the variance (and standard deviation) of Y be?
If you are using a sensor that gives a large error, how would you best use it?

65. Linear combinations (3)
With d and α un-correlated => V[d, α] = 0 (co-variance is zero)
di is the mean value and d ~ N(0, σd)
αi is the mean value and α ~ N(0, σα)

66. Linear combinations (4)
D = {The total distance} is calculated as before as this is only the sum of all d’s
The expected value and the variance become:

67. Linear combinations (5)
 = {The heading angle} is calculated as before as this is only the sum of all ’s, i.e. as the sum of all changes in heading
The expected value and the variance become:
What if we want to predict X and Y from our measured d’s and ’s?

68. Non-linear combinations (1)
X(N) is the previous value of X plus the latest movement (in the X direction)
The estimate of X(N) becomes:
This equation is non-linear as it contains the term:
and for X(N) to become Gaussian distributed, this equation must be replaced with a linear approximation around To do this we can use the Taylor expansion of the first order. By this approximation we also assume that the error is rather small!
With perfectly known N-1 and N-1 the equation would have been linear!

69. Non-linear combinations (2)
Use a first order Taylor expansion and linearize X(N) around
This equation is linear as all error terms are multiplied by constants and we can calculate the expected value and the variance as we did before.

70. Non-linear combinations (3)
The variance becomes (calculated exactly as before):
Two really important things should be noticed, first the linearization only affects the calculation of the variance and second (which is even more important) is that the above equation is the partial derivatives of:
with respect to our uncertain parameters squared multiplied with their variance!

71. Non-linear combinations (4)
This result is very good => an easy way of calculating the variance => the law of error propagation
The partial derivatives of become:

72. Non-linear combinations (5)
The plot shows the variance of X for the time step 1, …, 20 and as can be noticed the variance (or standard deviation) is constantly increasing.
d = 1/10
 = 5/360

73. The Error Propagation Law

74. The Error Propagation Law

75. The Error Propagation Law

76. Multidimensional Gaussian distributions MGD (1)
The Gaussian distribution can easily be extended for several dimensions by: replacing the variance () by a co-variance matrix () and the scalars (x and mX) by column vectors.
The CVM describes (consists of):
1) the variances of the individual dimensions => diagonal elements
2) the co-variances between the different dimensions => off-diagonal elements
! Symmetric
! Positive definite

77. An n-d Gaussian
distribution is given by:
A 1-d Gaussian
distribution is given by:

78. MGD (2) Eigenvalues => standard deviations
Eigenvectors => rotation of the ellipses

79. MGD (3)
The co-variance between two stochastic variables is calculated as:
Which for a discrete variable becomes:
And for a continuous variable becomes:

80. MGD (4) - Non-linear combinations
The state variables (x, y, ) at time k+1 become:

81. MGD (5) - Non-linear combinations
We know that to calculate the variance (or co-variance) at time step k+1 we must linearize Z(k+1) by e.g. a Taylor expansion - but we also know that this is done by the law of error propagation, which for matrices becomes:
With fX and fU are the Jacobian matrices (w.r.t. our uncertain variables) of the state transition matrix.

82. MGD (6) - Non-linear combinations
The uncertainty ellipses for X and Y (for time step ) is shown in the figure.

83. Circular Error Problem
If we have a map:
We can localize!
NOT THAT SIMPLE!
If we can localize:
We can make a map!

84. Expectation-Maximization (EM)
Algorithm
Initialize: Make random guess for lines
Repeat:
Find the line closest to each point and group into two sets. (Expectation Step)
Find the best-fit lines to the two sets (Maximization Step)
Iterate until convergence
The algorithm is guaranteed to converge to some local optima

85. Example:

86. Example:

87. Example:

88. Example:

89. Example:
Converged!

90. Probabilistic Mapping
Maximum Likelihood Estimation
E-Step: Use current best map and data to find belief probabilities
M-step: Compute the most likely map based on the probabilities computed in the E-step.
Alternate steps to get better map and localization estimates
Convergence is guaranteed as before.

91. but computed backward in time
The E-Step
P(st|d,m) = P(st |o1, a1… ot,m) . P(st |at…oT,m)
t  Bel(st)
Markov Localization t
Analogous to 
but computed backward in time

92. The M-Step # of times l was observed at <x,y>
Updates occupancy grid
P(mxy=l | d) =
# of times l was observed at <x,y>
# of times something was obs. at <x,y>

93. Probabilistic Mapping
Addresses the Simultaneous Mapping and Localization problem (SLAM)
Robust
Hacks for easing computational and processing burden
Caching
Selective computation
Selective memorization

97. Markov Assumption Future is Independent of Past Given Current State
“Assume Static World”

98. Probabilistic Model
Action Data
Observation Data

99. Derivation : Markov Localization
Bayes
Markov
Total Probability
Markov

100. Mobile Robot Localization
Proprioceptive Sensors: (Encoders, IMU) - Odometry, Dead reckoning
Exteroceptive Sensors: (Laser, Camera) - Global, Local Correlation
Scan-Matching
Scan 1
Scan 2
Iterate
Displacement Estimate
Initial Guess
Point Correspondence
Scan-Matching
Correlate range measurements to estimate displacement
Can improve (or even replace) odometry – Roumeliotis TAI-14
Previous Work - Vision community and Lu & Milios [97]

101. Explicit models of uncertainty & noise sources for each scan point:
Weighted Approach
Explicit models of uncertainty & noise sources for each scan point:
Sensor noise & errors
Range noise
Angular uncertainty
Bias
Point correspondence uncertainty
Correspondence Errors
Combined
Uncertanties
Improvement vs. unweighted method:
More accurate displacement estimate
More realistic covariance estimate
Increased robustness to initial conditions
Improved convergence
1 m
x500

102. Goal: Estimate displacement (pij ,fij )
Weighted Formulation
Goal: Estimate displacement (pij ,fij )
Measured range data from poses i and j
sensor noise
bias
true range
{uk} = measured data
{rk} = true range measurements
{duk} = zero mean gaussian noise process
{bk} = noise process
Error between kth scan point pair
= rotation of fij
Noise Error
Bias Error
Correspondence Error

103. Covariance of Error Estimate
Correspondence
Sensor Noise
Bias
Covariance of error between kth scan point pair =
Lik sq sl
Sensor Noise
Pose i
{uk} = measured data
{rk} = true range measurements
{duk} = zero mean gaussian noise process
{bk} = noise process
Assume correspondence erors noise and bias errors are mutually independent
Sensor Bias
neglect for now

104. Correspondence Error = cijk
Estimate bounds of cijk from the geometry
of the boundary and robot poses
Max error
Assume uniform distribution
where

105. Finding incidence angles aik and ajk
Hough Transform
-Fits lines to range data
-Local incidence angle estimated from line tangent and scan angle
-Common technique in vision community (Duda & Hart [72])
-Can be extended to fit simple curves
aik
Scan Points
Fit Lines

106. Maximum Likelihood Estimation
Likelihood of obtaining errors {eijk} given displacement
Non-linear Optimization Problem
Position displacement estimate obtained in closed form
Orientation estimate found using 1-D numerical
optimization, or series expansion approximation methods

107. Weighted vs. Unweighted matching of two poses
Experimental Results
Weighted vs. Unweighted matching of two poses
512 trials with different initial displacements within :
+/- 15 degrees of actual angular displacement
+/- 150 mm of actual spatial displacement
Initial Displacements
Unweighted Estimates
Weighted Estimates
Increased robustness to inaccurate initial displacement guesses
Fewer iterations for convergence

108. Unweighted Weighted

109. Displacement estimate errors at end of path
Eight-step, 22 meter path
Displacement estimate errors at end of path
Odometry = 950mm
Unweighted = 490mm
Weighted = 120mm
More accurate covariance estimate
Improved knowledge of
measurement uncertainty
- Better fusion with other sensors

110. Uncertainty From Sensor Noise and Correspondence Error
x500