ROBOT MAPPING AND EKF SLAM

Slides:



Advertisements
Similar presentations
Mobile Robot Localization and Mapping using the Kalman Filter
Advertisements

EKF, UKF TexPoint fonts used in EMF.
Probabilistic Robotics
Probabilistic Robotics
Probabilistic Robotics SLAM. 2 Given: The robot’s controls Observations of nearby features Estimate: Map of features Path of the robot The SLAM Problem.
(Includes references to Brian Clipp
Robot Localization Using Bayesian Methods
The GraphSLAM Algorithm Daniel Holman CS 5391: AI Robotics March 12, 2014.
IR Lab, 16th Oct 2007 Zeyn Saigol
Lab 2 Lab 3 Homework Labs 4-6 Final Project Late No Videos Write up
Probabilistic Robotics
Markov Localization & Bayes Filtering 1 with Kalman Filters Discrete Filters Particle Filters Slides adapted from Thrun et al., Probabilistic Robotics.
Observers and Kalman Filters
Kalman Filter CMPUT 615 Nilanjan Ray. What is Kalman Filter A sequential state estimator for some special cases Invented in 1960’s Still very much used.
Introduction to Mobile Robotics Bayes Filter Implementations Gaussian filters.
Probabilistic Robotics: Kalman Filters
Adam Rachmielowski 615 Project: Real-time monocular vision-based SLAM.
Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.
Introduction to Kalman Filter and SLAM Ting-Wei Hsu 08/10/30.
CS 547: Sensing and Planning in Robotics Gaurav S. Sukhatme Computer Science Robotic Embedded Systems Laboratory University of Southern California
SLAM: Simultaneous Localization and Mapping: Part I Chang Young Kim These slides are based on: Probabilistic Robotics, S. Thrun, W. Burgard, D. Fox, MIT.
Probabilistic Robotics
Discriminative Training of Kalman Filters P. Abbeel, A. Coates, M
Probabilistic Robotics: Motion Model/EKF Localization
Probabilistic Robotics
Stanford CS223B Computer Vision, Winter 2007 Lecture 12 Tracking Motion Professors Sebastian Thrun and Jana Košecká CAs: Vaibhav Vaish and David Stavens.
Stanford CS223B Computer Vision, Winter 2006 Lecture 11 Filters / Motion Tracking Professor Sebastian Thrun CAs: Dan Maynes-Aminzade, Mitul Saha, Greg.
Estimation and the Kalman Filter David Johnson. The Mean of a Discrete Distribution “I have more legs than average”
Course AE4-T40 Lecture 5: Control Apllication
Probabilistic Robotics Bayes Filter Implementations Gaussian filters.
Kalman Filtering Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics TexPoint fonts used in EMF. Read.
SLAM: Simultaneous Localization and Mapping: Part II BY TIM BAILEY AND HUGH DURRANT-WHYTE Presented by Chang Young Kim These slides are based on: Probabilistic.
Overview and Mathematics Bjoern Griesbach
Slam is a State Estimation Problem. Predicted belief corrected belief.
EKF and UKF Day EKF and RoboCup Soccer simulation of localization using EKF and 6 landmarks (with known correspondences) robot travels in a circular.
Kalman filter and SLAM problem
Itamar Kahn, Thomas Lin, Yuval Mazor
Markov Localization & Bayes Filtering
/09/dji-phantom-crashes-into- canadian-lake/
Computer vision: models, learning and inference Chapter 19 Temporal models.
From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun.
Computer Vision Group Prof. Daniel Cremers Autonomous Navigation for Flying Robots Lecture 6.2: Kalman Filter Jürgen Sturm Technische Universität München.
3D SLAM for Omni-directional Camera
Probabilistic Robotics Robot Localization. 2 Localization Given Map of the environment. Sequence of sensor measurements. Wanted Estimate of the robot’s.
Probabilistic Robotics Bayes Filter Implementations Gaussian filters.
Probabilistic Robotics Bayes Filter Implementations.
Young Ki Baik, Computer Vision Lab.
Human-Computer Interaction Kalman Filter Hanyang University Jong-Il Park.
Mobile Robot Localization (ch. 7)
Real-Time Simultaneous Localization and Mapping with a Single Camera (Mono SLAM) Young Ki Baik Computer Vision Lab. Seoul National University.
1 Assignment, Project and Presentation Mobile Robot Localization by using Particle Filter by Chong Wang, Chong Fu, and Guanghui Luo. Tracking, Mapping.
Unscented Kalman Filter 1. 2 Linearization via Unscented Transform EKF UKF.
An Introduction To The Kalman Filter By, Santhosh Kumar.
Tracking with dynamics
Extended Kalman Filter
Nonlinear State Estimation
The Unscented Kalman Filter for Nonlinear Estimation Young Ki Baik.
Probabilistic Robotics Bayes Filter Implementations Gaussian filters.
Probabilistic Robotics Bayes Filter Implementations Gaussian filters.
Using Sensor Data Effectively
PSG College of Technology
Unscented Kalman Filter
Simultaneous Localization and Mapping
Probabilistic Robotics
A Short Introduction to the Bayes Filter and Related Models
Unscented Kalman Filter
Probabilistic Robotics
Extended Kalman Filter
Kalman Filtering COS 323.
Extended Kalman Filter
Presentation transcript:

ROBOT MAPPING AND EKF SLAM

Slides for the book: Probabilistic Robotics Authors: Sebastian Thrun Wolfram Burgard Dieter Fox Publisher: MIT Press, 2005. Web site for the book & more slides: http://www.probabilistic-robotics.org/

Landmark-based Localization

Extended Kalman Filters

Nonlinear Dynamic Systems

Non-Gaussian Distributions in Kalman filters?

We use Taylor expansion to linearize Last slide main observation We use Taylor expansion to linearize

First Order Taylor Expansions

Reminder: Jacobian Matrix nonlinear x=(x1,x2,…,xn) Tangent plane represents multi-dimensional derivative

Reminder: Jacobian Matrix Jacobian Matrix represents the gradient of a scalar valued function

EKF Linearization is done through First Order Taylor Expansion Linearization for prediction Linearization for correction

matrix vector vector Matrix-vector multiplication again

) ( ) When we insert to Kalman filter equations ( nonlinear From previous slide ( ) ( ) When we insert to Kalman filter equations

When we insert to Kalman filter equations nonlinear From previous slide ( ) When we insert to Kalman filter equations

EKF Linearization: First Order Taylor Series Expansion EKF Linearization: First Order Taylor Series Expansion. Matrix vs Scalar Scalar case MATRIX case

EKF Algorithm compared with Kalman Algorithm Was in linear Kalman

Extended Kalman Filter Algorithm

Linearity Assumption Revisited Grey represents true distribution of p(y) Linearity Assumption Revisited Linear mapping of Mean: y=ax+b This is ideal case of pushing gaussian through a linear system Mean of p(y) Mean of p(x)

This is a real case of pushing a gaussian through a non-linear system Grey represents true distribution of p(y) Non-linear Function Non-linear function g(x) This is a real case of pushing a gaussian through a non-linear system Gaussian of p(y) We are approximating grey by blue, not good Mean of p(x)

EKF Linearization (1): Taylor approximation and EKF Gaussian This example shows that Gaussian of EKF better represents estimated value than the Gaussian mean Better than in last slide. The mean is closer to grey shape

EKF Linearization (2) Now the height is reduced to 2. More uncertainty EKF even better

EKF Linearization (3) Even more difference of EKF Gaussian and real Gaussian, but better estimation Smaller max value

Conclusion: NON-GAUSSIAN distribution of output requires EKF or something better that would calculate the mean better In this case the input Gaussian does not create an output Gaussian! And Gaussian estimation of output is worse than EKF estimation of Gaussian with respect to mean Gaussian with high value of standard deviation

In this case EKF works similarly to KF EKF Linearization (5) Gaussian similar to EKF gaussian In this case EKF works similarly to KF Narrow and short gaussian

Summary on Extended Kalman Filters

Literature on Kalman Filter and EKF

EKF Localization Example

Localization of two-dimensional robot “Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91] Given Map of the environment. Sequence of sensor measurements. Wanted Estimate of the robot’s position. Problem classes Position tracking Global localization Kidnapped robot problem (recovery)

EKF_localization ( mt-1, St-1, ut, zt, m): Prediction Phase: (x,y,) Jacobian of g w.r.t location velocity Velocity and radial velocity are controls Radial velocity Jacobian of g w.r.t control Motion noise Predicted mean Predicted covariance

EKF_localization ( mt-1, St-1, ut, zt, m): Correction phase: r = range  = angle Predicted measurement mean Jacobian of h w.r.t location Pred. measurement covariance Kalman gain Updated mean Updated covariance

EKF Prediction Step for four cases Updated mean Updated covariance Predicted mean Predicted covariance

EKF Prediction Step for four cases: now we take other system Predicted mean Predicted covariance Updated mean Updated covariance

EKF Observation Prediction Step robot landmark Z = measurement Similarly for three other cases but sizes and shapes are different Predicted robot covariance

EKF Correction Step

Estimation Sequence (1) Based on measurement only Based on Kalman Initial true robot landmark Initial estimated robot

Estimation Sequence (2) And we see a difference between estimated and real positions of the robot

Comparison to GroundTruth

EKF Summary Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2) Not optimal! Can diverge if nonlinearities are large! Works surprisingly well even when all assumptions are violated!

SLAM Review

Noise in motion Noise in measurement

EKF SLAM

State vector has size 3+2n 3 coordinates for robot’s pose 2N coordinates for N features

State Representation for EKF SLAM Mean Value Matrix (Vector) Covariance matrix

State Representation for EKF SLAM Please observe how these matrices are partitioned to submatrices Covariance matrix Mean Value Matrix

Just another notation for the same stuff as in last slide

One more often used notation for the same stuff as in last slide

Filter Cycle for the EKF SLAM

In green we show the part related to state prediction

Now in green we have new estimated state and new estimated covariance matrix

A concrete example of EKF SLAM

EKF-SLAM: a concrete example of robot on a plane Robot moves in the plane Velocity-based motion model: V = speed  = radial velocity Robot observes point landmarks Range-bearing sensor Known data association to landmark points Known number of landmarks.

State Vector of mean estimates N landmarks Covariance matrix Nothing known about landmarks

Last slide Remember notations F T x and g of our matrices See next slide

Now our task is to calculate matrix G

Note that we denote the Jacobian of motion by Gxt

From previous slides This slides just shows how to calculate the Jacobian using a new notation

Noise in motion

Noise in measurement

Use notation F from one of previous slides

Now we have to calculate Jacobian H and calculate Kalman Gain Matrix K

Implementation Notes

Loop Closing

EKF SLAM Properties

Practical Examples of applications