TSRT14: Sensor Fusion Lecture 9

Similar documents
TSRT14: Sensor Fusion Lecture 8

TSRT14: Sensor Fusion Lecture 1

TSRT14: Sensor Fusion Lecture 6. Kalman Filter (KF) Le 6: Kalman filter (KF), approximations (EKF, UKF) Lecture 5: summary

Sensor Fusion, 2014 Lecture 1: 1 Lectures

SLAM Techniques and Algorithms. Jack Collier. Canada. Recherche et développement pour la défense Canada. Defence Research and Development Canada

Simultaneous Localization and Mapping

EKF and SLAM. McGill COMP 765 Sept 18 th, 2017

1 Kalman Filter Introduction

Particle Filters; Simultaneous Localization and Mapping (Intelligent Autonomous Robotics) Subramanian Ramamoorthy School of Informatics

CIS 390 Fall 2016 Robotics: Planning and Perception Final Review Questions

Introduction to Mobile Robotics SLAM: Simultaneous Localization and Mapping

9 Multi-Model State Estimation

Chapter 9: Bayesian Methods. CS 336/436 Gregory D. Hager

Introduction to Mobile Robotics Bayes Filter Particle Filter and Monte Carlo Localization

Bayes Filter Reminder. Kalman Filter Localization. Properties of Gaussians. Gaussians. Prediction. Correction. σ 2. Univariate. 1 2πσ e.

Robotics. Mobile Robotics. Marc Toussaint U Stuttgart

Manipulators. Robotics. Outline. Non-holonomic robots. Sensors. Mobile Robots

SLAM for Ship Hull Inspection using Exactly Sparse Extended Information Filters

From Bayes to Extended Kalman Filter

E190Q Lecture 11 Autonomous Robot Navigation

RAO-BLACKWELLIZED PARTICLE FILTER FOR MARKOV MODULATED NONLINEARDYNAMIC SYSTEMS

Mobile Robot Localization

ECE276A: Sensing & Estimation in Robotics Lecture 10: Gaussian Mixture and Particle Filtering

Lecture 4: Extended Kalman filter and Statistically Linearized Filter

Using the Kalman Filter for SLAM AIMS 2015

Modeling and state estimation Examples State estimation Probabilities Bayes filter Particle filter. Modeling. CSC752 Autonomous Robotic Systems

Probabilistic Fundamentals in Robotics. DAUIN Politecnico di Torino July 2010

Lecture 2: From Linear Regression to Kalman Filter and Beyond

Towards airborne seismic. Thomas Rapstine & Paul Sava

Lecture 6: Multiple Model Filtering, Particle Filtering and Other Approximations

Introduction to Unscented Kalman Filter

Introduction to Mobile Robotics Probabilistic Sensor Models

Autonomous Mobile Robot Design

2D Image Processing (Extended) Kalman and particle filter

Mobile Robot Localization

Estimating the Shape of Targets with a PHD Filter

A Comparison of the EKF, SPKF, and the Bayes Filter for Landmark-Based Localization

L11. EKF SLAM: PART I. NA568 Mobile Robotics: Methods & Algorithms

Parameter Estimation in a Moving Horizon Perspective

Sampling Based Filters

CS 532: 3D Computer Vision 6 th Set of Notes

The Unscented Particle Filter

Rao-Blackwellised posterior linearisation backward SLAM

Lecture 2: From Linear Regression to Kalman Filter and Beyond

Fisher Information Matrix-based Nonlinear System Conversion for State Estimation

SIMULTANEOUS STATE AND PARAMETER ESTIMATION USING KALMAN FILTERS

Lecture 7: Optimal Smoothing

For final project discussion every afternoon Mark and I will be available

Motion Tracking with Fixed-lag Smoothing: Algorithm and Consistency Analysis

COS Lecture 16 Autonomous Robot Navigation

Chapter 4 State Estimation

Method 1: Geometric Error Optimization

Image Alignment and Mosaicing Feature Tracking and the Kalman Filter

Robot Localisation. Henrik I. Christensen. January 12, 2007

Autonomous Navigation for Flying Robots

Probabilistic Fundamentals in Robotics. DAUIN Politecnico di Torino July 2010

RECURSIVE OUTLIER-ROBUST FILTERING AND SMOOTHING FOR NONLINEAR SYSTEMS USING THE MULTIVARIATE STUDENT-T DISTRIBUTION

Particle Filters. Pieter Abbeel UC Berkeley EECS. Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics

Lecture 22: Graph-SLAM (2)

Robotics 2 Target Tracking. Kai Arras, Cyrill Stachniss, Maren Bennewitz, Wolfram Burgard

4 Derivations of the Discrete-Time Kalman Filter

Probabilistic Fundamentals in Robotics

Kalman Filter. Predict: Update: x k k 1 = F k x k 1 k 1 + B k u k P k k 1 = F k P k 1 k 1 F T k + Q

Comparision of Probabilistic Navigation methods for a Swimming Robot

Sensor Fusion: Particle Filter

Local Probabilistic Models: Continuous Variable CPDs

On the Treatment of Relative-Pose Measurements for Mobile Robot Localization

Pose Tracking II! Gordon Wetzstein! Stanford University! EE 267 Virtual Reality! Lecture 12! stanford.edu/class/ee267/!

A Constant-Time Algorithm for Vector Field SLAM Using an Exactly Sparse Extended Information Filter

EXAMINATION IN TSRT14 SENSOR FUSION

Terrain Navigation Using the Ambient Magnetic Field as a Map

Bayesian Methods / G.D. Hager S. Leonard

L06. LINEAR KALMAN FILTERS. NA568 Mobile Robotics: Methods & Algorithms

FUNDAMENTAL FILTERING LIMITATIONS IN LINEAR NON-GAUSSIAN SYSTEMS

UAV Navigation: Airborne Inertial SLAM

A Tree Search Approach to Target Tracking in Clutter

Square Root Unscented Particle Filtering for Grid Mapping

Exact State and Covariance Sub-matrix Recovery for Submap Based Sparse EIF SLAM Algorithm

Lego NXT: Navigation and localization using infrared distance sensors and Extended Kalman Filter. Miguel Pinto, A. Paulo Moreira, Aníbal Matos

GMTI Tracking in the Presence of Doppler and Range Ambiguities

A Sequential Monte Carlo Approach for Extended Object Tracking in the Presence of Clutter

Parameterized Joint Densities with Gaussian Mixture Marginals and their Potential Use in Nonlinear Robust Estimation

Maximum Likelihood Diffusive Source Localization Based on Binary Observations

Instituto de Sistemas e Robótica * Pólo de Coimbra * Tracking and Data Association Using Data from a LRF

Data Fusion of Dual Foot-Mounted Zero Velocity Update (ZUPT) Aided Inertial Navigation Systems (INSs) using Centroid Method

Data assimilation with and without a model

Mini-Course 07 Kalman Particle Filters. Henrique Massard da Fonseca Cesar Cunha Pacheco Wellington Bettencurte Julio Dutra

System identification and sensor fusion in dynamical systems. Thomas Schön Division of Systems and Control, Uppsala University, Sweden.

(Extended) Kalman Filter

Lecture 13 Visual Inertial Fusion

Lecture 6: April 19, 2002

Cooperative relative robot localization with audible acoustic sensing

PATTERN RECOGNITION AND MACHINE LEARNING CHAPTER 13: SEQUENTIAL DATA

AUTOMOTIVE ENVIRONMENT SENSORS

2D Image Processing. Bayes filter implementation: Kalman filter

(W: 12:05-1:50, 50-N201)

2D Image Processing. Bayes filter implementation: Kalman filter

Censoring and Fusion in Non-linear Distributed Tracking Systems with Application to 2D Radar

Kalman filtering and friends: Inference in time series models. Herke van Hoof slides mostly by Michael Rubinstein

Aided Inertial Navigation With Geometric Features: Observability Analysis

Transcription:

TSRT14: Sensor Fusion Lecture 9 Simultaneous localization and mapping (SLAM) Gustaf Hendeby gustaf.hendeby@liu.se

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 1 / 28 Le 9: simultaneous localization and mapping (SLAM) Whiteboard: ˆ SLAM problem formulation ˆ Framewor for EKF-SLAM and fastslam (with PF and MPF) Slides: ˆ Algorithms ˆ Properties ˆ Examples and illustrations

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 2 / 28 Lecture 8: summary SIS PF Algorithm Choose the number of particles N, a proposal density q(x (i) x(i) 0: 1, y 1:), and a threshold N th (for instance N th = 2 3 N). ˆ Initialization: Generate x (i) 0 p x0, i = 1,..., N. Iterate for = 1, 2,... : 1. Measurement update: For i = 1, 2,..., N: w (i) w(i) p(y x (i) ), and normalize w(i). 2. Estimation: MMSE ˆx N i=1 w(i) x(i). 3. Resampling: Resample with replacement when 1 N eff = i < N th. (w(i) )2 4. Prediction: Generate samples x (i) +1 q(x x (i) update the weights w (i) +1 w(i) w (i) +1. p(x (i) x(i) 1 ) q(x (i) x(i) 1,y ) 1, y ),, and normalize

Simultaneous Localization and Mapping (SLAM)

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 4 / 28 SLAM: problem formulations Localization concerns the estimation of pose from nown landmars. Navigation concerns estimation of pose, velocities and other states from nown landmars. Mapping concerns the estimation of landmar positions from nown values of pose. SLAM concerns the joint estimation of pose and landmar positions. ˆ Variations on the same theme: Simultaneous navigation and mapping SNAM?! and Simultaneous tracing and mapping STAM?!

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 5 / 28 EKF SLAM: model ˆ Assume a linear(-ized) model x +1 = F x + Gv m +1 = m cov(v ) = Q y = H x x + H m (c1:i )m + e, cov(e ) = R. ˆ The map is represented by m. ˆ The index c 1:I relate the observed landmar i to a map landmar j i, which affects the measurement model. ˆ Association is crucial for some sensors (laser, radar, etc.), but less of a problem some applications (camera using image features, microphones using designed pings). ˆ The state and its covariance matrix ( ) ( ) ˆx P xx P xm ẑ =, P =. ˆm P mx P mm

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 6 / 28 Original SLAM Application ˆ Assume a ground robot with three states: x = (X, Y, ψ) T. ˆ Robot measures speed and turn rate: u = (v, ψ) T. ˆ Simple dynamics. ˆ Sensor: 1. Ranging sensor (sonar, laser scanner, radar) measures distance to obstacles (walls, furniture); tens to hundreds of landmars. 2. Vision (camera, Kinect, stereo camera) provides detections (corners, marers, patterns) as potential landmars; thousands or tens of thousands of landmars. ˆ P xx small matrix, P mx Approach? thin matrix and P mm large matrix. Both EKF and PF apply to the problem, but how to handle the large dimensions in the best way? Start with studying the basic EKF.

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 7 / 28 EKF SLAM: basic KF steps Time update: ( F 0 ẑ 1 = 0 I P 1 = Measurement update: S = H x P 1 xx HxT K x = ( H x P xx 1 + Hm P mx K m ) ẑ 1 1, ( F P xx 1 1 F T + G Q G T F P xm 1 1 P mx 1 1 F T + H m P 1 mm HmT ) 1 S 1 ) S 1 = ( H x P xm 1 + Hm P mm 1 ε = y H x ˆx 1 H m ) ε ẑ = ẑ 1 + ( K x K m P = P 1 ( K x K m ˆm 1 ) ( ) S 1 K x T K m P mm 1 1 + H m P 1 mx HxT + H x P 1 xm HmT + R )

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 8 / 28 EKF SLAM: KF problems ˆ All elements in P mm are affected by the measurement update. ˆ It turns out that the cross correlations are essential for performance. ˆ No simple turn-around.

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 9 / 28 EKF SLAM: information form ˆ Focus on sufficient statistics and information matrix ı l = I l ẑ l I l = P 1 l = ( P xx l P mx l ˆ Measurement update trivial P l xm P l mm ) 1 = ( I xx l I mx l I xm l I mm l ). ı = ı 1 + H T R 1 y I = I 1 +H T R 1 H. Note: The update is sparse!!!

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 10 / 28 EKF SLAM: information filter algorithm (1/4) Initialization: ı x 1 0 = 0 3 1 ı m 1 0 = 0 0 0 I xx 1 0 = 0 3 3 I mx 1 0 = 0 0 3 I mm 1 0 = 0 0 0 Note: The information form allows for representing no prior nowledge with zero information (infinite covariance).

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 11 / 28 EKF SLAM: information filter algorithm (2/4) 1. Associate a map landmar j = c i to each observed landmar j, and construct the matrix H m. This step includes data gating for outlier rejection and trac handling to start and end landmar tracs. 2. Measurement update: Note: ı x = ıx 1 + HxT ı m = ım 1 + HmT I xx = Ixx I xm = Ixm I mm 1 +HxT 1 +HxT = Imm 1 +HmT R 1 y R 1 y R 1 Hx R 1 Hm R 1 Hm H m is very thic, but contains mostly zeros. The low-ran sparse corrections influencing only a fraction of the matrix elements.

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 12 / 28 EKF SLAM: information filter algorithm (3/4) 3. Time update: Note: Ī xx 1 = F 1 Ī xm 1 = F 1 I xx 1 1 F T I xm 1 1 ( M = G G T F 1 I xx 1 1 F T I xx 1 = Īxx 1 Īxx 1M Ī xx 1 I xm 1 = Īxm 1 Īxx 1M Ī xm 1, I mm 1 = Īmm 1 Īmx 1M G T Īxm 1 ı x 1 = ( I I xx 1 G Q G T F T + Q 1 ) 1G T, ) ı x 1 1 ı m 1 = ım 1 1 Imx 1 G Q G T F T ı x 1 Now, I mm 1 is corrected with the inner product of Īxm 1 which gives a full matrix. Many of the elements in I xm 1 are close to zero and may be truncated!

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 13 / 28 EKF SLAM: information filter algorithm (4/4) 4. Estimation: P = I 1, ˆx = P xx ıx + P xm ım, ˆm = P mx ıx + P mm ım. Here is another catch, the information matrix needs to be inverted! The pose is needed at all times for linearization and data gating. How to proceed? Idea: Solve ı l = I l ẑ l, directly using a gradient search algorithm initialized at previous estimate.

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 14 / 28 EKF SLAM: summary ˆ EKF SLAM scales well in state dimension. ˆ EKF SLAM scales badly in landmar dimension, though natural approximations exist for the information form. ˆ EKF SLAM is not robust to incorrect associations.

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 15 / 28 FastSLAM: idea Basic factorization idea: p(x 1:, m y 1: ) = p(m x 1:, y 1: )p(x 1: y 1: ). ˆ The first factor corresponds to a classical mapping problem, and is solved by the (E)KF. ˆ The second factor is approximated by the PF. ˆ Leads to a marginalized PF (MPF) where each particle is a pose trajectory with an attached map corresponding to mean and covariance of each landmar, but no cross-correlations.

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 16 / 28 FastSLAM: mapping solution (1/3) Assume observation model linear(-ized) in landmar position 0 = h 0 (y n, x ) + h 1 (y n, x )m ln + en, cov(en ) = Rn. This formulation covers: ˆ First order Taylor expansions. ˆ Bearing and range measurements, where h i (y n, x ) has two rows per landmar in 2D SLAM. ˆ Bearing-only measurements coming from a camera detection.

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 17 / 28 FastSLAM: mapping solution (2/3) Linear estimation theory applies. WLS estimate: ˆm l = ( N =1 h1t (y n l, x )(R n l ) 1 h 1 (y n l, x ) }{{} I l N N ) 1 =1 h1t (y n l, x )(R n l ) 1 h 0 (y n l, x ) = (I l N) 1 ı l N. }{{} ı l N In this sum, h i is an empty matrix if the map landmar n does not get an associated observation landmar at time. Under a Gaussian noise assumption, the posterior distribution is Gaussian ( m l y 1:N, x 1:N ) N ( (I l N ) 1 ı l N, (I l N) 1).

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 18 / 28 FastSLAM: mapping solution (3/3) Kalman filter for mapping on information form ı l = ıl 1 + h1t (y n l, x )R 1 h0 (y n l, x ), I l = Il 1 +h1t (y n l, x )R 1 h1 (y n l, x ), ˆm l = (I l ) 1 ı l. Note the problem of inverting a large matrix to compute the landmar positions. Lielihood in the Gaussian case: p(y n l y 1: 1, x 1: ) = N ( h 0 (y n l, x )+h 1 (y n l, x ) ˆm l 1, Rn l +h1 (y n l, x )(I l ) 1 h 1T (y n l, x ) ).

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 19 / 28 FastSLAM: the algorithm (1/2) 1. Initialize the particles x (i) 1 p 0 (x), where N denotes the number of particles. 2. Data association that assigns a map landmar n l to each observed landmar l. Initialize new map landmars if necessary. 3. Importance weights w (i) = l N ( h 0 (y l, x )+h 1 (y l, x ) ˆm n l 1, Rl +h 1 (y l, x )(I n l ) 1 h 1T (y l, x ) ). where the product is taen over all observed landmars l, and normalize w (i) = w (i) / N j=1 w(j). 4. Resampling a new set of particles with replacement Pr(x (i) = x(j) ) = w(j), j = 1,..., N.

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 20 / 28 FastSLAM: the algorithm (2/2) 5. Map measurement update: p(m (i) x (i) 1:, y 1:) = N ( (I (i) ) 1 ı (i), (I(i) ) 1), 6. Pose time update: fastslam 1.0 (SIR PF) ı = ı 1 + h 1T (y, x )R 1 h0 (y, x ), I = I 1 +h 1T (y, x )R 1 h1 (y, x ). x (i) +1 p(x +1 x (i) 1: ). fastslam 2.0 (PF with optimal proposal) x (i) +1 p(x +1 x (i) 1:, y 1:+1) p(x +1 x (i) 1: )p(y +1 x +1, x (i) 1:, y 1:).

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 21 / 28 FastSLAM: summary FastSLAM is ideal for a ground robot with three states and vision sensors providing thousands of landmars. ˆ FastSLAM scales linearly in landmar dimension. ˆ As the standard PF, FastSLAM scales badly in the state dimension. ˆ FastSLAM is relatively robust to incorrect assocations, since associations are local for each particle and not global as in EKF-SLAM. MfastSLAM combines all the good landmars of the EKF SLAM and fastslam!

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 22 / 28 MFastSLAM: idea Factorization in three factors p(x P 1:, xj, m y 1: ) = p(m x j, xp 1:, y 1:)p(x j xp 1:, y 1:)p(x P 1: y 1:) corresponding to: ˆ one low-dimensional PF, ˆ one (E)KF attached to each particle, ˆ one WLS estimate to each particle and each map landmar.

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 23 / 28 SLAM Illustration Airborne simultanous localization and mapping (SLAM) using UAV with camera. Research collaboration with IDA. General idea: augment state vector with parameters representing the map. http://youtu.be/ha_nzeuoy9y http://youtu.be/vqlachl3yc4 Comparison of EKF and FastSLAM on same dataset.

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 23 / 28 SLAM Illustration Airborne simultanous localization and mapping (SLAM) using UAV with camera. Research collaboration with IDA. General idea: augment state vector with parameters representing the map. http://youtu.be/ha_nzeuoy9y http://youtu.be/vqlachl3yc4 Comparison of EKF and FastSLAM on same dataset.

TSRT14 Lecture 9 Gustaf Hendeby Ex: RSS SLAM (1/2) Prize winning MSc thesis at FOI 2014. Foot-mounted IMU for odometry. Opportunistic radio signals for fingerprinting. RSS at 95500 Hz Signal strength (dbm) 70 75 80 85 90 95 0 50 100 Samples 150 200 Spring 2018 24 / 28

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 25 / 28 Ex: RSS SLAM (2/2) ˆ Estimate the error in the odometry. ˆ Gaussian process to represent the map. ˆ Particle filter solution. 60 70 80 90 Y (m) 100 110 120 130 140 150 20 40 60 80 100 120 140 160 180 200 220 X (m) https://youtu.be/lkd2vwmfn00

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 25 / 28 Ex: RSS SLAM (2/2) ˆ Estimate the error in the odometry. ˆ Gaussian process to represent the map. ˆ Particle filter solution. 60 70 80 90 Y (m) 100 110 120 130 140 150 20 40 60 80 100 120 140 160 180 200 220 X (m) https://youtu.be/lkd2vwmfn00

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 26 / 28 SLAM Examples on Youtube ˆ LEGO Mindstorm ground robot with sonar http://youtu.be/7k8dzwqbssa ˆ Indoor mapping by UAV with laser scanner http://youtu.be/imsozupffu ˆ Indoor mapping using hand-held stereo camera with IMU at FOI http://youtu.be/7f-nqxmo1qe ˆ Intelligent vacuum cleaner using ceiling vision http://youtu.be/bq5hzzgf3vq ˆ App Ball Invasion uses augmented reality based on SLAM (KTH student did the SLAM implementation) http://youtu.be/whgtvdxtvz

Summary

TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 28 / 28 Lecture 9: summary Simultaneous Localization And Mapping (SLAM) Joint estimation of trajectory x1: and map parameters θ in sensor model y = h(x ; θ) + e. Algorithms: http://youtu.be/ha_nzeuoy9y NLS SLAM: iterate between filtering and mapping. EKF-SLAM: EKF (information form) on augmented state vector z = (xt, θt )T. FastSLAM: MPF on augmented state vector z = (xt, θt )T. http://youtu.be/vqlachl3yc4

Gustaf Hendeby hendeby@isy.liu.se www.liu.se

2024 © sciencedocbox.com Privacy Policy | Terms of Service | Feedback