Slam is a State Estimation Problem Predicted belief

  • Slides: 54
Download presentation
Slam is a State Estimation Problem

Slam is a State Estimation Problem

Predicted belief corrected belief

Predicted belief corrected belief

Bayes Filter Reminder

Bayes Filter Reminder

Gaussians

Gaussians

Standard deviation Covariance matrix

Standard deviation Covariance matrix

Gaussians in one and two dimensions One standard deviation two standard deviations

Gaussians in one and two dimensions One standard deviation two standard deviations

Multivariate probability Gaussians in three dimensions

Multivariate probability Gaussians in three dimensions

Properties of Gaussians for Univariate case Mean on output of linear system Standard deviation

Properties of Gaussians for Univariate case Mean on output of linear system Standard deviation on output of linear system Linear system For two-dimensional system:

Properties of Gaussians for Multivariate case From previous slide

Properties of Gaussians for Multivariate case From previous slide

Properties of Gaussians Important Property of all these methods

Properties of Gaussians Important Property of all these methods

Discrete Kalman Filters

Discrete Kalman Filters

Kalman Filter background 1. 2. 3. 4. 5. 6. 7. 8. Kalman Filter is

Kalman Filter background 1. 2. 3. 4. 5. 6. 7. 8. Kalman Filter is a Bayes Filter Kalman Filter uses Gaussians Estimator for the linear Gaussian case Optimal solution for linear models and Gaussian distributions Developed in late 1950’s Most relevant Bayes filter variant in practice Applications in econcomics, weather forecasting, satellite navigations, GPS, robotics, robot vision and many other Kalman filter is just few matrix operations such as multiplication.

Discrete Kalman Filter

Discrete Kalman Filter

Components of a Kalman Filter

Components of a Kalman Filter

Example of Kalman Filter Updates in one dimension Kalman Filter calculates a weighted mean

Example of Kalman Filter Updates in one dimension Kalman Filter calculates a weighted mean value!

Kalman Filter Updates in 1 D: PREDICTION Single dimension Again generalization to many dimensions

Kalman Filter Updates in 1 D: PREDICTION Single dimension Again generalization to many dimensions here Matrices in multi-dimensions

Kalman Filter Updates in 1 D: CORRECTION Variant single variable Generalization: Variant of multiple

Kalman Filter Updates in 1 D: CORRECTION Variant single variable Generalization: Variant of multiple variables matrix

Kalman Filter Updates

Kalman Filter Updates

Linear Gaussian Systems

Linear Gaussian Systems

Linear Gaussian Systems: Initialization • Initial belief has a normal distribution:

Linear Gaussian Systems: Initialization • Initial belief has a normal distribution:

Linear Gaussian Systems: Dynamics Gaussian

Linear Gaussian Systems: Dynamics Gaussian

Linear Gaussian Systems: Dynamics From previous slide Linear, gaussian

Linear Gaussian Systems: Dynamics From previous slide Linear, gaussian

Linear Gaussian Systems: Observations R = correction

Linear Gaussian Systems: Observations R = correction

Linear Gaussian Systems: Observations

Linear Gaussian Systems: Observations

Properties: Marginalization and Conditioning Notation for Gaussians All are Gaussian

Properties: Marginalization and Conditioning Notation for Gaussians All are Gaussian

Kalman Filter assumes linearity Zero-mean Gaussian Noise

Kalman Filter assumes linearity Zero-mean Gaussian Noise

Linear Motion Model We want to calculate this probability variable

Linear Motion Model We want to calculate this probability variable

Theorem 1 We want to calculate this probability variable

Theorem 1 We want to calculate this probability variable

We want to calculate this probability variable

We want to calculate this probability variable

Theorem 2 We want to calculate this probability variable

Theorem 2 We want to calculate this probability variable

Everything stays Gaussian: the belief is Gaussian! Theorem 3 • Proofs of these theorems

Everything stays Gaussian: the belief is Gaussian! Theorem 3 • Proofs of these theorems and properties are not trivial and can be found in the book by ‘three Germans” called Probabilistic Robotics

Kalman Filter Algorithm

Kalman Filter Algorithm

The Kalman Filter Assumptions are: 1. 2. 3. 4. Gaussian distributions Gaussian noise Linear

The Kalman Filter Assumptions are: 1. 2. 3. 4. Gaussian distributions Gaussian noise Linear motion Linear observation model Discuss later

Prediction of multidimensional mean Prediction of multidimensional covariance matrix Calculates multidimensional mean and covariance

Prediction of multidimensional mean Prediction of multidimensional covariance matrix Calculates multidimensional mean and covariance matrix Prediction phase R for motion Correction phase Q for measurement Kalman Calculates corrected multidimensional mean and covariance matrix

Kalman Filter Algorithm Different notation to previous slide Measurement noise

Kalman Filter Algorithm Different notation to previous slide Measurement noise

Kalman Filter Algorithm: navigation using odometry and measurement to landmark Predicted and corrected position

Kalman Filter Algorithm: navigation using odometry and measurement to landmark Predicted and corrected position of the ship

The Prediction-Correction-Cycle The phase of Prediction

The Prediction-Correction-Cycle The phase of Prediction

The Prediction-Correction-Cycle The phase of Correction

The Prediction-Correction-Cycle The phase of Correction

The Prediction-Correction-Cycle

The Prediction-Correction-Cycle

The general Optimal State Estimation Problem

The general Optimal State Estimation Problem

Diagram of general State Estimation 1 2 3 2 or 3 !

Diagram of general State Estimation 1 2 3 2 or 3 !

Discrete Kalman Filter This is what we discussed

Discrete Kalman Filter This is what we discussed

Linear-Optimal State Estimation Change with time derivative Compare with this

Linear-Optimal State Estimation Change with time derivative Compare with this

Linear-Optimal State Estimation (Kalman-Bucy Filter) Similar to before Kalman

Linear-Optimal State Estimation (Kalman-Bucy Filter) Similar to before Kalman

Estimation Gain for the Kalman-Bucy Filter • Same equations as those that define control

Estimation Gain for the Kalman-Bucy Filter • Same equations as those that define control gain, except – solution matrix, P, propagated forward in time – Matrices and matrix sequences are different

Second-Order Example of Kalman. Bucy Filter

Second-Order Example of Kalman. Bucy Filter

Second-Order Example of Kalman-Bucy Filter

Second-Order Example of Kalman-Bucy Filter

Kalman-Bucy Filter with Two Measurements

Kalman-Bucy Filter with Two Measurements

State Estimate with Angle Measurement Only

State Estimate with Angle Measurement Only

Kalman Filter Summary

Kalman Filter Summary

Non-Linear Dynamic Systems

Non-Linear Dynamic Systems

Sources • • Wolfram Burgard Cyrill Stachniss, Maren Bennewitz Kal Arras

Sources • • Wolfram Burgard Cyrill Stachniss, Maren Bennewitz Kal Arras