Slam is a State Estimation Problem

Predicted belief corrected belief

Bayes Filter Reminder

Gaussians

Standard deviation Covariance matrix

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

Gaussians in three dimensions Multivariate probability

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

Properties of Gaussians Properties of Gaussians for Multivariate case From previous slide

Properties of Gaussians Important Property of all these methods

Discrete Kalman Filters

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

Discrete Kalman Filter

Components of a Kalman Filter

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

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

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

Kalman Filter Updates

Linear Gaussian Systems

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

Dynamics Linear Gaussian Systems: Dynamics Gaussian

Linear Gaussian Systems: Dynamics From previous slide Linear, gaussian

Linear Gaussian Systems: Observations R = correction

Linear Gaussian Systems: Observations

: Marginalization and Conditioning Properties: Marginalization and Conditioning Notation for Gaussians All are Gaussian

Kalman Filter assumes linearity Zero-mean Gaussian Noise

Linear Motion Model We want to calculate this probability variable

Theorem 1

We want to calculate this probability variable

Theorem 2

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

Kalman Filter Algorithm

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

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

Kalman Filter Algorithm Different notation to previous slide Measurement noise

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 Correction

The Prediction-Correction-Cycle

The general Optimal State Estimation Problem

Diagram of general State Estimation or 3 !

Discrete Kalman Filter This is what we discussed

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

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 gain, except – solution matrix, P, propagated forward in time – Matrices and matrix sequences are different

Second-Order Example of Kalman- Bucy Filter

Kalman-Bucy Filter with Two Measurements

State Estimate with Angle Measurement Only

Kalman Filter Summary

Non-Linear Dynamic Systems

Sources Wolfram Burgard Cyrill Stachniss, Maren Bennewitz Kal Arras