Probability in Robotics

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

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

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

Uncertainty Representation

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

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

Axioms of Probability Theory Pr(A) denotes probability that proposition A is true. 1) 2) 3)

A Closer Look at Axiom 3 B

Using the Axioms

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.

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

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)

Law of Total Probability Discrete case Continuous case

Bayes Formula

Normalization

Conditioning Total probability: Bayes rule and background knowledge:

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

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!

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.

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

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

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.

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?

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

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.

Example: Closing the door

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.

Integrating the Outcome of Actions Continuous case: Discrete case:

Example: The Resulting Belief

How all of this relates to Sensors Sensor fusion

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

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

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

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.

Estimate of the expected value and the variance from observations

Linear combinations (1) X2 ~ N(m2, σ2) X1 ~ N(m1, σ1) Y ~ N(m1 + m2, sqrt(σ1 +σ2)) This property that Y remains Gaussian if the s.v. are combined linearily is one of the great properties of the Gaussian distribution!

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?

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

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:

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?

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!

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.

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!

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:

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

The Error Propagation Law

The Error Propagation Law

The Error Propagation Law

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

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

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

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

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

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.

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

Probabilistic Action model p(st|at-1,st-1) st-1 st-1 at-1 at-1 Continuous probability density Bel(st) after moving 40m (left figure) and 80m (right figure). Darker area has higher probablity.

Localization Localization without knowledge of start location

Localization Initial state detects nothing: Moves and detects landmark: Moves and detects nothing: Moves and detects landmark:

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

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

Example:

Example:

Example:

Example:

Example: Converged!

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.

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

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>

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

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

Probabilistic Model Action Data Observation Data

Derivation : Markov Localization Bayes Markov Total Probability Markov

Nature of Sensor Data: Uncertainty Range Data Odometry Data

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]

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

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

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

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

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

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

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

Unweighted Weighted

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

Conclusions Possible Future Work: Developed general approach to incorporate uncertainty into scan-match displacement estimates. range sensor error models novel correspondence error modeling Method can likely be extended to other range sensors (stereo cameras, radar, ultrasound, etc.) requires some specific sensor error models Showed that accurate error modelling can significantly improve displacement and covariance estimates as well as robustness Possible Future Work: Weighted correspondence for 3D feature matching

Uncertainty From Sensor Noise and Correspondence Error x500