Autonomous Mobile Robot Design

Autonomous Mobile Robot Design Topic: EKF-based SLAM Dr. Kostas Alexis (CSE)

These slides have partially relied on the course of C. Stachniss, Robot Mapping - WS 2013/14 Autonomous Robot Challenges Where am I? What is my environment?

Simultaneous Localization and Mapping Building a map and locate the robot in the map at the same time Chicken or Egg problem Map Localize

Definition of the SLAM Problem Given: The robot s controls Observations Wanted: Map of the environment Path of the robot

Recall: The Bayes filter concept For each time step, do: Apply motion model: Apply sensor model: η is a normalization factor to ensure that the probability is maximum 1.

EKF for Online SLAM The EKF provides a solution to the online SLAM problem, i.e.: Find the latest pose of the robot

Recall: The Extended Kalman Filter

Recall: The Extended Kalman Filter Prediction Step Correction Step

Recall: The Extended Kalman Filter EKF works for nonlinear models Prediction Step Correction Step

Recall: The Extended Kalman Filter Process noise Prediction Step Correction Step

Recall: The Extended Kalman Filter Prediction Step Kalman Gain: how certain the robot for its predicted belief and its measurements? Correction Step

Recall: The Extended Kalman Filter Prediction Step Sensor noise Correction Step

Recall: The Extended Kalman Filter Prediction Step Correction Step

EKF SLAM concept Application of the EKF to SLAM Estimate robot s pose and location of features in the environment Assumption: feature correspondence is known State space parametrization:

Assume a simple 3DoF robot The pose of a simplified robot in the 2D configuration space is defined by its x-y coordinates and the heading angle θ. The heading angle of the robot affects its dynamic trajectory in the x-y space. Let us defined the rotation matrix:

EKF SLAM: State representation Map with n landmarks: (3+2n)-dimensional Gaussian Belief is represented through the covariance matrix

EKF SLAM: State representation Compact representation of the covariance matrix

EKF SLAM: State representation More compactly (and simplifying notation as ):

EKF SLAM: Filter Cycle 1. State Prediction 2. Measurement Prediction 3. Measurement 4. Data Association 5. Update

EKF SLAM: State Prediction

EKF SLAM: Measurement Prediction

EKF SLAM: Obtained Measurement

EKF SLAM: Data Association data association and distance calculation

EKF SLAM: Update Step

EKF-SLAM: Comprehensive Example Setup: Robot moves in the 2D plane Velocity-based motion model Robot observes point landmarks (x,y) Range-bearing sensor Known data association Known-fixed number of landmarks

Initialization Robot starts in its own reference coordinate frame (all landmarks unknown) 2N+3 dimensions

EKF Algorithm

Prediction Step (motion) Goal: Update state space based on the robot s motion (Velocity-based) Robot motion on the plane

Prediction Step (motion) Goal: Update state space based on the robot s motion (Velocity-based) Robot motion on the plane How can this be mapped to the 2N+3 dimensional space?

Update the State Space From the motion on the plane To the 2N+3 dimensional space

EKF Algorithm Done!

Update Covariance The function g only affects the robot s motion and not the landmarks Jacobian of the motion (3x3) Identity (2Nx2N)

Update Covariance The function g only affects the robot s motion and not the landmarks Jacobian of the motion (3x3) Identity (2Nx2N) The landmark positions don t change during the prediction step

Jacobian of the Motion Calculate the Jacobian: no dependency on x and y (velocity-based model)

Then this moves us to the update Done!

Then this moves us to the update Done! not updated during the prediction step

EKF Algorithm Done! Done!

EKF SLAM - Prediction Maps from the low-dim space to the high-dim space

EKF Algorithm Done! Apply & Done!

Indexing of measurements EKF SLAM - Correction Known data association : i-th measurement observes the landmark with index j Initialize landmark if unobserved Compute the expected observation Compute the Jacobian of h Then, proceed with computing the Kalman Gain

Range-Bearing Observation Range-bearing observation If landmark has not been observed observed location of landmark j estimated robot s location relative measurement

Expected Observation Compute expected observation according to the current estimate Where the robot expects to see the landmark

Jacobian of the Observation Based on: Compute the Jacobian: low-dim space

Jacobian of the Observation Use the computed Jacobian: Map it to the high dimensional space

Next steps Done! Done!

Next steps Done! Done! Apply & Done Apply & Done Apply & Done

EKF SLAM Correction [1/2]

EKF SLAM Correction [2/2]

Implementation Notes Measurement update in a single step requires only one full belief update Always normalize the angular components

almost

Loop Closing Recognizing an already mapped area Data association with High ambiguity Possible environmental symmetries Uncertainties collapse after a loop closure (even if the loop closure was not correct )

Before the Loop Closure

After the Loop Closure

Code Examples and Tasks Conduct Camera Calibration MATLAB: https://github.com/unrarl/autonomous_mobile_robot_design_course/tree/master/matlab /localization-mapping/ekf-mono-slam

How does this apply to my project? To estimate the pose of the robot and the map within the environment that it navigates

Find out more http://webdiis.unizar.es/~neira/pdfs/freesubnet%20%20ekf%20slam.pdf http://ranger.uta.edu/~gianluca/astra/gustavopuerto/introductiontoestimat ionfornavigation_newman06.pdf http://www.iri.upc.edu/people/jsola/joansola/objectes/curs_slam/slam2d /SLAM%20course.pdf Course of C. Stachniss, Robot Mapping - WS 2013/14

Thank you! Please ask your question!