Probabilistic Fundamentals in Robotics Gaussian Filters Basilio Bona DAUIN Politecnico di Torino July 2010
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 July 2010 2
Basic mathematical framework Recursive state estimation Basic concepts in probability bbl Robot environment Bayes filters Gaussian filters (parametric filters) Kl Kalman filter Extended Kalman Filter UnscentedKalman filter Information filter Nonparametric filters Histogram filter Particle filter Basilio Bona July 2010 3
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 July 2010 4
Multi variate Gaussian distribution Covariance matrix Mean vector Basilio Bona July 2010 5
Examples Bi dimensional Gaussian with conditional probabilities Mixture of Gaussians Basilio Bona July 2010 6
Covariance matrix Basilio Bona July 2010 7
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 July 2010 8
Kalman filter (2) Basilio Bona July 2010 9
Kalman filter (3) Basilio Bona July 2010 10
Kalman filter (4) Basilio Bona July 2010 11
Kalman filter algorithm (1) Prediction Kalman gain Innovation (residuals) covariance Update Basilio Bona July 2010 12
Block diagram z t u t B t μ + t + C t zˆt + At μt 1 D μt + K + t residuals Basilio Bona July 2010 13
Kalman filter algorithm (2) visible hidden Basilio Bona July 2010 14
Kalman filter example Initial state measurement update prediction measurement update Basilio Bona July 2010 15
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 July 2010 16
Linear transformation of Gaussians Basilio Bona July 2010 17
Extended Kalman Filter (EKF) When the linearity assumptions do not hold (as in robot motion models dlor orientation i models) dl) a closed dform solution of the predicted belief does not exists Nonlinear state & measurement equations ExtendedKalman Filter (EKF) approximates the nonlinear transformations with a linear one Linearization is performed around the most likely value: i.e., the mean value Basilio Bona July 2010 18
EKF Example Montecarlo generated distribution Transformed mean value Approximating mean value Approximating Gaussian Approximating Gaussian uses mean and covariance of the Montecarlo generated distribution Basilio Bona July 2010 19
EKF Example Basilio Bona July 2010 20
EKF Example Approximating Gaussian: the normal distribution built using mean and covariance of the true nonlinear distributions Approximating Gaussian EKF Gaussian EKF Gaussian: the normal distribution built using mean and covariance of the true nonlinear distributions Basilio Bona July 2010 21
EKF linearization Taylor expansion Depends only on the mean Basilio Bona July 2010 22
EKF algorithm Basilio Bona July 2010 23
KF vs EKF Basilio Bona July 2010 24
Features EKF is a very popular tool for state estimation in robotics It has the same timecomplexity 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 July 2010 25
Uncertainty More uncertain More uncertain Less uncertain Less uncertain Basilio Bona July 2010 26
Uncertainty More uncertain Less uncertain Basilio Bona July 2010 27
Nonlinearity More nonlinear More linear Basilio Bona July 2010 28
Nonlinearity More nonlinear More linear Basilio Bona July 2010 29
Example: EKF Localization within a sensor infrastructure Fixed sensors (deployed in known positions inside the environment) Mobile bl Robot can acquire odometric measurements and distance information from sensors in known positions True position of the mobile robot t=0 i KF estimate (time zero)
Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry t=0
Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry Filter Prediction t=0 Luca Carlone Politecnico di Torino
Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry Filter Prediction Acquire meas. t=0 Luca Carlone Politecnico di Torino
Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry Filter Prediction Acquire meas. Filter Update t=0 Luca Carlone Politecnico di Torino
Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry Filter Prediction Acquire meas. Filter Update STEP 2: Acquire odometry t=0 Luca Carlone Politecnico di Torino
Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry Filter Prediction Acquire meas. Filter Update STEP 2: Acquire odometry Filter Prediction t=0 Luca Carlone Politecnico di Torino
Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry Filter Prediction Acquire meas. Filter Update STEP 2: Acquire odometry Filter Prediction Acquire meas. t=0 Luca Carlone Politecnico di Torino
Example: EKF Localization within a sensor infrastructure t=0 STEP 1: Acquire odometry Filter Prediction Acquire meas. Filter Update STEP 2: Acquire odometry Filter Prediction Acquire meas. Filter Update Luca Carlone Politecnico di Torino
Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry Filter Prediction Acquire meas. Filter Update STEP 2: Acquire odometry Filter Prediction Acquire meas. Filter Update t=0... Luca Carlone Politecnico di Torino
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 transformeddistribution 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 July 2010 40
UKF Basilio Bona July 2010 41
UKF Basilio Bona July 2010 42
UKF Basilio Bona July 2010 43
UKF Algorithm part a) Basilio Bona July 2010 44
UKF Algorithm part b) Cross covariance Basilio Bona July 2010 45
EKF vs UKF Basilio Bona July 2010 46
EKF vs UKF Basilio Bona July 2010 47
KF EKF UKF KF EKF UKF Basilio Bona July 2010 48
Information filters Belief is represented by Gaussians Moments parameterization Canonical parameterization KF EKF UKF Duality IF EIF Mean Information vector Covariance Information matrix Basilio Bona July 2010 49
Multivariate normal distribution Basilio Bona July 2010 50
Mahalanobis distance Mahalanobis distance Same Euclidean distance Same Mahalanobis distance Basilio Bona July 2010 51
IF algorithm Basilio Bona July 2010 52
IF vs KF IF Prediction i step requires two matrix inversion KF Prediction i step is additive i Measurements update dt is additive Measurements update requires matrix inversion Duality Basilio Bona July 2010 53
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 State estimate Basilio Bona July 2010 54
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 order) 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 July 2010 55
Final comments In many problems the interaction between state variable is local l=> structure t 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 July 2010 56
Basilio Bona July 2010 57
Basilio Bona July 2010 58
Basilio Bona July 2010 59
Basilio Bona July 2010 60
Basilio Bona July 2010 61
Basilio Bona July 2010 62
Basilio Bona July 2010 63
Basilio Bona July 2010 64
Basilio Bona July 2010 65
Basilio Bona July 2010 66
Basilio Bona July 2010 67
Basilio Bona July 2010 68
Basilio Bona July 2010 69
Basilio Bona July 2010 70
Basilio Bona July 2010 71
Basilio Bona July 2010 72
Basilio Bona July 2010 73
Basilio Bona July 2010 74
Thank you. Any question? Basilio Bona July 2010 75
Thrun Slides on Kalman Filters
PhD_course_2010 Outline.pptx Basilio Bona July 2010 77