Slam is a State Estimation

Problem

Predicted belief

corrected belief

Bayes Filter Reminder

Gaussians

Standard deviation

Covariance matrix

Gaussians in one and two dimensionsOne 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 GaussiansProperties of Gaussians for Multivariate case

From previous slide

Properties of GaussiansImportant Property of all these methods

Discrete Kalman Filters

Kalman Filter background1. Kalman Filter is a Bayes Filter2. Kalman Filter uses Gaussians3. Estimator for the linear Gaussian case4. Optimal solution for linear models and Gaussian

distributions5. Developed in late 1950’s6. Most relevant Bayes filter variant in practice7. Applications in econcomics, weather forecasting, satellite

navigations, GPS, robotics, robot vision and many other8. Kalman filter is just few matrix operations such as

multiplication.

Discrete Kalman Filter

Components of a Kalman Filter

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

Kalman Filter Updates in 1D: CORRECTION

Variant single variable Generalization:

Variant of multiple variables

matrix

Kalman Filter Updates

Linear Gaussian Systems

Linear Gaussian Systems: Initialization

• Initial belief has a normal distribution:

Linear Gaussian Systems: Dynamics

Gaussian

Linear Gaussian Systems: DynamicsFrom previous slide

Linear, gaussian

Linear Gaussian Systems: Observations

R = correction

Linear Gaussian Systems: Observations

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

We want to calculate this probability variable

Theorem 1

We want to calculate this probability variable

We want to calculate this probability variable

Theorem 2

Everything stays Gaussian: the belief is Gaussian!

• 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 distributions2. Gaussian noise3. Linear motion4. 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

EstimationProblem

Diagram of general State Estimation

123

2 or 3 !

Discrete Kalman Filter

This is what we discussed

Linear-Optimal State Estimation

Compare with this

Change with timederivative

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

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