ROBOTICS 01PEEQW Basilio Bona DAUIN Politecnico di Torino - - PowerPoint PPT Presentation

ROBOTICS 01PEEQW

Basilio Bona DAUIN – Politecnico di Torino

Probabilistic Fundamentals in Robotics

Gaussian Filters

Course Outline

Basic mathematical framework Probabilistic models of mobile robots Mobile robot localization problem Robotic mapping Probabilistic planning and control Reference textbook Thrun, Burgard, Fox, “Probabilistic Robotics”, MIT Press, 2006 http://www.probabilistic-robotics.org/

Basilio Bona 3

Basic mathematical framework

Recursive state estimation

Basic concepts in probability Robot environment Bayes filters

Gaussian filters (parametric filters)

Kalman filter Extended Kalman Filter Unscented Kalman filter Information filter

Nonparametric filters

Histogram filter Particle filter

Basilio Bona 4

Introduction

Gaussian filters are different implementations of Bayes filters for continuous spaces, with specific assumptions on probability distributions Beliefs are represented by multi-variate normal distributions

Basilio Bona 5

Multi-variate Gaussian distribution

Basilio Bona 6

Mean vector Covariance matrix

Examples

Basilio Bona 7

Mixture of Gaussians Bi-dimensional Gaussian with conditional probabilities

Covariance matrix

Basilio Bona 8

Kalman filter (1)

Kalman filter (KF) [Swerling: 1958, Kalman: 1960] applies to linear Gaussian systems KF computes the belief for continuous states governed by linear dynamic state equations Beliefs are expressed by normal distributions KF is not applicable to discrete or hybrid state space systems

Basilio Bona 9

Kalman filter (2)

Basilio Bona 10

Kalman filter (3)

Basilio Bona 11

Kalman filter (4)

Basilio Bona 12

Kalman filter algorithm (1)

Basilio Bona 13

Prediction Update

Kalman gain Innovation (residuals) covariance

Block diagram

Basilio Bona 14

+

t

µ ˆ

t

z

t

B

residuals +

t

A D

+ +

t

K

t

C

+ −

1 t−

µ

t

µ

t

z

t

u

Kalman filter algorithm (2)

Basilio Bona 15

visible hidden

update update

Kalman filter example

Basilio Bona 16

measurement measurement prediction Initial state

From Kalman filter to extended Kalman filter

Kalman filter is based on linearity assumptions Gaussian random variables are expressed by means and covariance matrices of normal distributions Gaussian distributions are transformed into Gaussian distributions Kalman filter is optimal Kalman filter is efficient

Basilio Bona 17

Linear transformation of Gaussians

Basilio Bona 18

Extended Kalman Filter (EKF)

When the linearity assumptions do not hold (as in robot motion models or orientation models) a closed form solution of the predicted belief does not exists

Basilio Bona 19

Extended Kalman Filter (EKF) approximates the nonlinear transformations with a linear

• ne

Linearization is performed around the most likely value: i.e., the mean value

Nonlinear state & measurement equations

EKF Example

Basilio Bona 20

Transformed mean value Approximating Gaussian Approximating mean value Approximating Gaussian uses mean and covariance of the Montecarlo generated distribution Montecarlo generated distribution

EKF Example

Basilio Bona 21

EKF Example

Basilio Bona 22

EKF Gaussian Approximat ing Gaussian

Approximating Gaussian: the normal distribution built using mean and covariance

• f the true nonlinear

distributions EKF Gaussian: the normal distribution built using mean and covariance

• f the true nonlinear

distributions

EKF linearization

Taylor expansion

Basilio Bona 23

Depends only on the mean

EKF algorithm

Basilio Bona 24

KF vs EKF

Basilio Bona 25

Features

EKF is a very popular tool for state estimation in robotics It has the same time complexity of the KF It is robust and simple Limitations: rarely state and measurement functions are linear. Goodness of linear approximation depends on

Degree of uncertainty Degree of nonlinearity

When using EKF the uncertainty must be kept small as much as possible

Basilio Bona 26

Uncertainty

Basilio Bona 27

More uncertain Less uncertain More uncertain Less uncertain

Uncertainty

Basilio Bona 28

Less uncertain More uncertain

Nonlinearity

Basilio Bona 29

More nonlinear More linear

Nonlinearity

Basilio Bona 30

More nonlinear More linear

t=0

Fixed sensors (deployed in known positions inside the environment) True position of the mobile robot KF estimate (time zero)

Mobile Robot can acquire odometric measurements and distance information from sensors in known positions

Example: EKF Localization within a sensor infrastructure

Basilio Bona 31

t=0

STEP 1:

• Acquire
• dometry

Example: EKF Localization within a sensor infrastructure

Basilio Bona 32

STEP 1:

• Acquire
• dometry
• Filter Prediction

Luca Carlone – Politecnico di Torino

Example: EKF Localization within a sensor infrastructure

Basilio Bona 33

t=0

STEP 1:

• Acquire
• dometry
• Filter Prediction
• Acquire meas.

Luca Carlone – Politecnico di Torino

Example: EKF Localization within a sensor infrastructure

Basilio Bona 34

t=0

STEP 1:

• Acquire
• dometry
• Filter Prediction
• Acquire meas.
• Filter Update

Luca Carlone – Politecnico di Torino

Example: EKF Localization within a sensor infrastructure

Basilio Bona 35

t=0

STEP 1:

• Acquire
• dometry
• Filter Prediction
• Acquire meas.
• Filter Update

STEP 2:

• Acquire
• dometry

Luca Carlone – Politecnico di Torino

Example: EKF Localization within a sensor infrastructure

Basilio Bona 36

t=0

STEP 1:

• Acquire
• dometry
• Filter Prediction
• Acquire meas.
• Filter Update

STEP 2:

• Acquire
• dometry
• Filter Prediction

Luca Carlone – Politecnico di Torino

Example: EKF Localization within a sensor infrastructure

Basilio Bona 37

t=0

STEP 1:

• Acquire
• dometry
• Filter Prediction
• Acquire meas.
• Filter Update

STEP 2:

• Acquire
• dometry
• Filter Prediction
• Acquire meas.

Luca Carlone – Politecnico di Torino

Example: EKF Localization within a sensor infrastructure

Basilio Bona 38

t=0

STEP 1:

• Acquire
• dometry
• Filter Prediction
• Acquire meas.
• Filter Update

STEP 2:

• Acquire
• dometry
• Filter Prediction
• Acquire meas.
• Filter Update

Luca Carlone – Politecnico di Torino

Example: EKF Localization within a sensor infrastructure

Basilio Bona 39

t=0

STEP 1:

• Acquire
• dometry
• Filter Prediction
• Acquire meas.
• Filter Update

STEP 2:

• Acquire
• dometry
• Filter Prediction
• Acquire meas.
• Filter Update

. . .

Luca Carlone – Politecnico di Torino

Example: EKF Localization within a sensor infrastructure

Basilio Bona 40

t=0

Unscented Kalman Filter (UKF)

UKF performs a stochastic linearization based on a weighted statistical linear regression A deterministic sampling technique (the unscented transform) is used to pick a minimal set of sample points (sigma points) around the mean value of the normal pdf The sigma points are propagated through the nonlinear functions, and then used to compute the mean and covariance of the transformed distribution This approach

removes the need to explicitly compute Jacobians, which for complex functions can be difficult to calculate produces a more accurate estimate of the posterior distribution

Basilio Bona 41

UKF

Basilio Bona 42

UKF

Basilio Bona 43

UKF

Basilio Bona 44

UKF Algorithm – part a)

Basilio Bona 45

UKF Algorithm – part b)

Basilio Bona 46

Cross covariance

EKF vs UKF

Basilio Bona 47

EKF vs UKF

Basilio Bona 48

KF – EKF – UKF

Basilio Bona 49

KF EKF UKF

Information filters

Basilio Bona 50

Belief is represented by Gaussians Moments parameterization Canonical parameterization

KF – EKF – UKF IF – EIF Mean Covariance Information vector Information matrix Duality

Multivariate normal distribution

Basilio Bona 51

Mahalanobis distance

Basilio Bona 52

Mahalanobis distance

Same Euclidean distance Same Mahalanobis distance

IF algorithm

Basilio Bona 53

IF vs KF

IF

Prediction step requires two matrix inversion

• Measurements update is

additive

• KF

Prediction step is additive

• Measurements update

requires matrix inversion

• Basilio Bona

54

Duality

Extended information filter – EIF

It is similar to EKF and applies when state and measurement equations are nonlinear Jacobians G and H replace A, B and C matrices

Basilio Bona 55

State estimate

Practical considerations

IF advantages over KF:

Simpler global uncertainty representation: set Ω = 0 Numerically more stable (in many but not all robotics applications) Integrates information in simpler way Is naturally fit for multi-robot problems (decentralized data integration => Bayes rule => logarithmic form => addition of terms => arbitrary

• rder)

IF limitations:

A state estimation is required (inversion of a matrix) Other matrix inversions are necessary (not required for EKF) Computationally inferior to EKF for high-dim state spaces

Basilio Bona 56

Final comments

In many problems the interaction between state variable is local => structure on Ω => sparseness of Ω but not of Σ Information filters as graphs: sparse information matrix = sparse graph Such graphs are known as Gaussian Markov random fields

Basilio Bona 57

Basilio Bona 58

Basilio Bona 59

Basilio Bona 60

Basilio Bona 61

Basilio Bona 62

Basilio Bona 63

Basilio Bona 64

Basilio Bona 65

Basilio Bona 66

Basilio Bona 67

Basilio Bona 68

Basilio Bona 69

Basilio Bona 70

Basilio Bona 71

Basilio Bona 72

Basilio Bona 73

Basilio Bona 74

Basilio Bona 75

Basilio Bona 76

Thank you. Any question?