1 of 22

Aerial Robotics

State Estimation: SLAM Extended Kalman Filter

C. Papachristos

Robotic Workers (RoboWork) Lab

University of Nevada, Reno

CS-491/691

2 of 22

State Estimation

Localization / Mapping

​

  • Remember:
    • Localization – Self-localize and estimate own pose w.r.t. a map of the environment
    • Mapping – Build a map representation of the environment

​

  • Unbiased Map is required to perform robot Localization
  • Accurate Location estimate is required to building a Map of the environment

​

  • Simultaneous Localization And Mapping (SLAM):
    • No a priori knowledge of the environment Map
    • No externally provided knowledge of the robot Location

​

  • Robot has to estimate own pose alongside building a map

​

​

​

CS491/691 C. Papachristos

3 of 22

Simultaneous Localization And Mapping

SLAM – with a Filter

​

SLAM Approach:

  • Pick natural scene “features” as landmarks,�observe their motion & reason about robot motion

​

    • Features must be “distinctive”
    • Visual: Points, Lines, Patches
    • Range: 3D Points, 3D Lines, Planes, Corners

​

Use internal representations for

    • Landmarks positions (the Map)
    • Sensor parameters - e.g. Camera

​

Assumption

    • Robot’s uncertainty initialized�at some given value – e.g. zero uncertainty

​

​

​

​

​

CS491/691 C. Papachristos

4 of 22

Simultaneous Localization And Mapping

SLAM – with a Filter

​

Recursively & in Real-time:

​

    • Predict how the robot has moved
    • Extract Observations
    • Update the internal representations

CS491/691 C. Papachristos

5 of 22

Simultaneous Localization And Mapping

SLAM – with a Filter

​

Recursively & in Real-time:

​

    • Predict how the robot has moved
    • Extract Observations
    • Update the internal representations

​

​

  • Robot extracts observations (features), mapped with uncertainty related to the measurement model
    • E.g. pinhole camera model (mapping of world point into image coordinates)

CS491/691 C. Papachristos

6 of 22

Simultaneous Localization And Mapping

SLAM – with a Filter

​

Recursively & in Real-time:

​

    • Predict how the robot has moved
    • Extract Observations
    • Update the internal representations

​

​

  • Time passes (and) robot moves,�pose uncertainty increases as per the motion model
    • E.g. driving around – uncertainty is added due to odometric wheel slippage etc.

CS491/691 C. Papachristos

7 of 22

Simultaneous Localization And Mapping

SLAM – with a Filter

​

Recursively & in Real-time:

​

    • Predict how the robot has moved
    • Extract Observations
    • Update the internal representations

​

​

  • Robot extracts more observations (features), …

CS491/691 C. Papachristos

8 of 22

Simultaneous Localization And Mapping

SLAM – with a Filter

​

Recursively & in Real-time:

​

    • Predict how the robot has moved
    • Extract Observations
    • Update the internal representations

​

​

  • Robot extracts more observations (features), their uncertainty results from the combination of
    • the measurement uncertainty
    • the robot pose uncertainty

​

  • Map becomes correlated to the robot pose estimate

CS491/691 C. Papachristos

9 of 22

Simultaneous Localization And Mapping

SLAM – with a Filter

​

Recursively & in Real-time:

​

    • Predict how the robot has moved
    • Extract Observations
    • Update the internal representations

​

​

  • More time passes (and) robot moves even more,�pose uncertainty increases as per the motion model
    • E.g. keeps driving around – additional uncertainty is added due to odometric wheel slippage etc.

CS491/691 C. Papachristos

10 of 22

Simultaneous Localization And Mapping

SLAM – with a Filter

​

Recursively & in Real-time:

​

    • Predict how the robot has moved
    • Extract Observations
    • Update the internal representations

​

​

  • Robot re-observes an old feature,�“Loop-Closure detection” …

CS491/691 C. Papachristos

11 of 22

Simultaneous Localization And Mapping

SLAM – with a Filter

​

Recursively & in Real-time:

​

    • Predict how the robot has moved
    • Extract Observations
    • Update the internal representations

​

​

  • Robot re-observes an old feature,�“Loop-Closure detection”

​

  • Robot updates own pose estimate, which becomes correlated with the feature location estimates
    • Location & Map uncertainties become more “densely” correlated and “shrink”

CS491/691 C. Papachristos

12 of 22

Simultaneous Localization And Mapping

 

CS491/691 C. Papachristos

13 of 22

Simultaneous Localization And Mapping

SLAM – Probabilistic Formulation

​

Graphical Representation of Dynamic Bayesian Network of the SLAM process:

​

CS491/691 C. Papachristos

14 of 22

Simultaneous Localization And Mapping

SLAM – with a Filter

​

Remember: Recursively & in Real-time:

​

    • Predict how the robot has moved
    • Extract Observations
    • Update the internal representations

​

​

​

Core Principle

Eliminate all past poses

  • “Summarize” all experience w.r.t. last pose, using the covariance matrix over the associated state vector��Note: State vector encompasses map landmarks

CS491/691 C. Papachristos

15 of 22

Simultaneous Localization And Mapping

SLAM – with the Extended Kalman Filter (EKF)

​

Remember: Normality Assumptions – Multivariate Gaussians

​

​

  • Tracks a Gaussian belief of the state/landmarks

Assumes all noise is Gaussian

Follows the “predict/measure/update” approach

​

Advantages

    • Real-time online implementable
    • Performs well for Gaussian-like perturbations/uncertainty

​

Disadvantages

    • Unimodal estimate
    • States must be well approximated by a Gaussian
    • Computational scaling with larger maps, long sequences

CS491/691 C. Papachristos

16 of 22

Simultaneous Localization And Mapping

 

 

 

 

CS491/691 C. Papachristos

17 of 22

Simultaneous Localization And Mapping

 

 

 

 

Motion model based on integration of wheel arcs

“Virtual”�inputs

model

CS491/691 C. Papachristos

18 of 22

Simultaneous Localization And Mapping

SLAM – with the Extended Kalman Filter (EKF)

​

  • Motion model:

​

​

​

​

Jacobians

​

​

​

​

​

​

 

 

 

CS491/691 C. Papachristos

19 of 22

Simultaneous Localization And Mapping

 

 

 

“Innovation” : Predicted measurement vs Actual

 

 

 

“Kalman Gain”

CS491/691 C. Papachristos

20 of 22

Simultaneous Localization And Mapping

SLAM – with the Extended Kalman Filter (EKF)

​

  • Measurement model:

​

​

​

​

Jacobians

​

​

​

​

​

​

 

 

 

CS491/691 C. Papachristos

21 of 22

Simultaneous Localization And Mapping

 

 

 

CS491/691 C. Papachristos

22 of 22

Time for Questions !

CS-491/691

CS491/691 C. Papachristos