TSRT14: Sensor Fusion Lecture 9

Transcription

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

2 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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

3 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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

4 Simultaneous Localization and Mapping (SLAM)

5 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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?!

6 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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

7 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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.

8 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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 H m P 1 mx HxT + H x P 1 xm HmT + R )

9 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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.

10 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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!!!

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

12 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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.

13 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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!

14 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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.

15 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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.

16 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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.

17 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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.

18 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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).

19 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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 ) ).

20 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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.

21 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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:).

22 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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!

23 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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.

24 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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. Comparison of EKF and FastSLAM on same dataset.

25 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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. Comparison of EKF and FastSLAM on same dataset.

26 TSRT14 Lecture 9 Gustaf Hendeby Ex: RSS SLAM (1/2) Prize winning MSc thesis at FOI Foot-mounted IMU for odometry. Opportunistic radio signals for fingerprinting. RSS at Hz Signal strength (dbm) Samples Spring / 28

27 TSRT14 Lecture 9 Gustaf Hendeby Spring / 28 Ex: RSS SLAM (2/2) ˆ Estimate the error in the odometry. ˆ Gaussian process to represent the map. ˆ Particle filter solution Y (m) X (m)

28 TSRT14 Lecture 9 Gustaf Hendeby Spring / 28 Ex: RSS SLAM (2/2) ˆ Estimate the error in the odometry. ˆ Gaussian process to represent the map. ˆ Particle filter solution Y (m) X (m)

29 TSRT14 Lecture 9 Gustaf Hendeby Spring / 28 SLAM Examples on Youtube ˆ LEGO Mindstorm ground robot with sonar ˆ Indoor mapping by UAV with laser scanner ˆ Indoor mapping using hand-held stereo camera with IMU at FOI ˆ Intelligent vacuum cleaner using ceiling vision ˆ App Ball Invasion uses augmented reality based on SLAM (KTH student did the SLAM implementation)

30 Summary

31 TSRT14 Lecture 9 Gustaf Hendeby Spring / 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: 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.

32 Gustaf Hendeby