Probabilistic Fundamentals in Robotics

Transcript

1 Probabilistic Fundamentals in Robotics Gaussian filters Basilio Bona DAUIN Politecnico di Torino June 2011 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, robotics.org/ Basilio Bona June

2 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 June Introduction Gaussian filters are a class of Bayes filters implementations for random variables ibl dfi defined din continuous spaces, with ih specific assumptions on probability distributions Beliefs are represented by multivariate normal distributions Basilio Bona June

3 Multivariate Gaussian distribution Covariance matrix Mean vector Basilio Bona June Examples Bi dimensional Gaussian with conditional probabilities Mixture of Gaussians Basilio Bona June

4 Covariance matrix Basilio Bona June 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 June

5 Kalman filter (2) Normalization factor Basilio Bona June Kalman filter (3) Basilio Bona June

6 Kalman filter (4) Basilio Bona June Kalman filter algorithm (1) Prediction Kalman gain Innovation (residuals) covariance Update Basilio Bona June

7 Block diagram z t u t B t μ + t + C t zˆt + A t μt 1 D μt + K + t residuals Basilio Bona June Kalman filter algorithm (2) Visible layer Hidden layer t Basilio Bona June

8 Kalman filter example Initial state measurement update prediction measurement update Basilio Bona June From Kalman Filter to Extended Kalman Filter Kalman filter is based on linearity assumptions Gaussian random variables are expressed using only the means and covariance matrices of normal distributions Gaussian distributions are transformed into Gaussian distributions (linearity principle) KF is optimal KF is efficient Basilio Bona June

9 Linear transformation of Gaussians Gaussian into gaussian Basilio Bona June 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 Nonlinear state & measurement equations Extended Kalman Filter (EKF) approximates the nonlinear transformations with a linearized one Linearization is performed around the most likely value: i.e., the mean value Basilio Bona June

10 EKF Example Montecarlo generated distribution Transformed mean value Approximating mean value Approximating Gaussian Approximating Gaussian uses mean value and covariance of the Montecarlo generated distribution Basilio Bona June EKF Example Basilio Bona June

11 EKF Example Enlarged map Approximating Gaussian: the normal distribution is built using mean and covariance of the true nonlinear distributions Approximating Gaussian EKF Gaussian EKF Gaussian: the normal distribution is built using mean and covariance of the true nonlinear distributions Basilio Bona June EKF linearization Taylor expansion Depends only on the mean Basilio Bona June

12 EKF algorithm Basilio Bona June KF vs EKF Basilio Bona June

13 Features In robotics EKF is a very popular tool for state estimation It has the same time complexity of the KF Itis robust and simple Limitations: rarely state and measurement functions are linear, so it would be better to explicitly consider nonlinearities Goodness of linear approximation depends on Degree of uncertainty Degree of nonlinearity When using EKF, the uncertainty must be kept as small as possible Basilio Bona June Uncertainty sensitivity More uncertain More uncertain Less uncertain ti Less uncertain Basilio Bona June

14 Uncertainty sensitivity More uncertain Less uncertain Basilio Bona June Nonlinearity sensitivity More nonlinear More linear Basilio Bona June

15 Nonlinearity sensitivity More nonlinear More linear Basilio Bona June Example: EKF Localization within a sensor infrastructure Fixed sensors (deployed in known positions inside the environment) Mobile Robot can acquire odometric measurements and distance information from sensors in known positions True position of the mobile robot t=0 KF estimate (time zero) Basilio Bona June

16 Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry t=0 Basilio Bona June Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry Filter Prediction t=0 Basilio Bona June 2011 Luca Carlone Politecnico di Torino 32 16

17 Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry Filter Prediction Acquire meas. t=0 Basilio Bona June 2011 Luca Carlone Politecnico di Torino 33 Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry Filter Prediction Acquire meas. Filter Update t=0 Basilio Bona June 2011 Luca Carlone Politecnico di Torino 34 17

18 Example: EKF Localization within a sensor infrastructure STEP 1: Acquire odometry Filter Prediction Acquire meas. Filter Update STEP 2: Acquire odometry t=0 Basilio Bona June 2011 Luca Carlone Politecnico di Torino 35 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 Basilio Bona June 2011 Luca Carlone Politecnico di Torino 36 18

19 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 Basilio Bona June 2011 Luca Carlone Politecnico di Torino 37 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 Basilio Bona June 2011 Luca Carlone Politecnico di Torino 38 19

20 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... Basilio Bona June 2011 Luca Carlone Politecnico di Torino 39 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 obtain produces a more accurate estimate of the posterior distribution Basilio Bona June

21 UKF Basilio Bona June UKF Basilio Bona June

22 UKF Basilio Bona June UKF Algorithm part a) Basilio Bona June

23 UKF Algorithm part b) Cross covariance Basilio Bona June EKF vs UKF Basilio Bona June

24 EKF vs UKF Basilio Bona June KF EKF UKF KF EKF UKF Basilio Bona June

25 Information filters Belief is represented by Gaussians Moments parameterization Canonical parameterization KF EKF UKF Duality IF EIF Mean Covariance Information vector Information matrix Basilio Bona June Multivariate normal distribution Basilio Bona June

26 Mahalanobis distance Mahalanobis distance Same Euclidean distance Same Mahalanobis distance Basilio Bona June IF algorithm Basilio Bona June

27 IF vs KF IF The prediction step requires two matrix inversion KF The prediction step is additive Measurements update is additive Measurements update requires matrix inversion Duality Basilio Bona June 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 Basilio Bona June

28 Practical considerations IF advantages over KF: Simpler initial uncertainty representation: set Ω = 0 Numerically more stable, in many but not all robotics applications Integrates information in a simpler way Is naturally fit for multirobot 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 dimensional state spaces Basilio Bona June 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 June